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

Pull staging/IIO driver updates from Greg KH: "Here is the large set of staging and IIO driver changes for 5.8-rc1 Nothing major, but a lot of new IIO drivers are included in here, along with other core iio cleanups and changes. On the staging driver front, again, nothing noticable. No new deletions or additions, just a ton of tiny cleanups all over the tree done by a lot of different people. Most coding style, but many actual real fixes and cleanups that are nice to see. All of these have been in linux-next for a while with no reported issues" * tag 'staging-5.8-rc1' of git://git.kernel.org/pub/scm/linux/kernel/git/gregkh/staging: (618 commits) staging: rtl8723bs: Use common packet header constants staging: sm750fb: Add names to proc_setBLANK args staging: most: usb: init return value in default path of switch/case expression staging: vchiq: Get rid of VCHIQ_SERVICE_OPENEND callback reason staging: vchiq: move vchiq_release_message() into vchiq staging: vchi: Get rid of C++ guards staging: vchi: Get rid of not implemented function declarations staging: vchi: Get rid of vchiq_status_to_vchi() staging: vchi: Get rid of vchi_service_set_option() staging: vchi: Merge vchi_msg_queue() into vchi_queue_kernel_message() staging: vchiq: Move copy callback handling into vchiq staging: vchi: Get rid of vchi_queue_user_message() staging: vchi: Get rid of vchi_service_destroy() staging: most: usb: use function sysfs_streq staging: most: usb: add missing put_device calls staging: most: usb: use correct error codes staging: most: usb: replace code to calculate array index staging: most: usb: don't use error path to exit function on success staging: most: usb: move allocation of URB out of critical section staging: most: usb: return 0 instead of variable ...
author: Linus Torvalds <torvalds@linux-foundation.org> 2020-06-07 10:45:08 -0700
committer: Linus Torvalds <torvalds@linux-foundation.org> 2020-06-07 10:45:08 -0700
commit: 80ef846e9909f22ccdc2a4a6d931266cecce8b2c (patch)
tree: 0dd29cff6e6b4ee38452fb73ad292203c3ddeabf /drivers
parent: 081096d98bb23946f16215357b141c5616b234bf (diff)
parent: 77f55d1305c11fb729b88f2c3f7881ba0831fa6f (diff)
315 files changed, 12079 insertions, 8671 deletions
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 5d91a6dda894..1080637ca40e 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -89,13 +89,13 @@ config ADXL372_I2C
 	  module will be called adxl372_i2c.
 
 config BMA180
-	tristate "Bosch BMA180/BMA25x 3-Axis Accelerometer Driver"
-	depends on I2C
+	tristate "Bosch BMA023/BMA1x0/BMA25x 3-Axis Accelerometer Driver"
+	depends on I2C && INPUT_BMA150=n
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
-	  Say Y here if you want to build a driver for the Bosch BMA180 or
-	  BMA25x triaxial acceleration sensor.
+	  Say Y here if you want to build a driver for the Bosch BMA023, BMA150
+	  BMA180, SMB380, or BMA25x triaxial acceleration sensor.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called bma180.
@@ -238,7 +238,7 @@ config IIO_ST_ACCEL_3AXIS
 	  Say yes here to build support for STMicroelectronics accelerometers:
 	  LSM303DLH, LSM303DLHC, LIS3DH, LSM330D, LSM330DL, LSM330DLC,
 	  LIS331DLH, LSM303DL, LSM303DLM, LSM330, LIS2DH12, H3LIS331DL,
-	  LNG2DM, LIS3DE, LIS2DE12
+	  LNG2DM, LIS3DE, LIS2DE12, LIS2HH12
 
 	  This driver can also be built as a module. If so, these modules
 	  will be created:
diff --git a/drivers/iio/accel/bma180.c b/drivers/iio/accel/bma180.c
index fcd91d5f05fd..265722e4b13f 100644
--- a/drivers/iio/accel/bma180.c
+++ b/drivers/iio/accel/bma180.c
@@ -7,6 +7,7 @@
  * Support for BMA250 (c) Peter Meerwald <pmeerw@pmeerw.net>
  *
  * SPI is not supported by driver
+ * BMA023/BMA150/SMB380: 7-bit I2C slave address 0x38
  * BMA180: 7-bit I2C slave address 0x40 or 0x41
  * BMA250: 7-bit I2C slave address 0x18 or 0x19
  * BMA254: 7-bit I2C slave address 0x18 or 0x19
@@ -33,6 +34,8 @@
 #define BMA180_IRQ_NAME "bma180_event"
 
 enum chip_ids {
+	BMA023,
+	BMA150,
 	BMA180,
 	BMA250,
 	BMA254,
@@ -48,7 +51,7 @@ struct bma180_part_info {
 	unsigned int num_scales;
 	const int *bw_table;
 	unsigned int num_bw;
-	int center_temp;
+	int temp_offset;
 
 	u8 int_reset_reg, int_reset_mask;
 	u8 sleep_reg, sleep_mask;
@@ -57,13 +60,25 @@ struct bma180_part_info {
 	u8 power_reg, power_mask, lowpower_val;
 	u8 int_enable_reg, int_enable_mask;
 	u8 int_map_reg, int_enable_dataready_int1_mask;
-	u8 softreset_reg;
+	u8 softreset_reg, softreset_val;
 
 	int (*chip_config)(struct bma180_data *data);
 	void (*chip_disable)(struct bma180_data *data);
 };
 
 /* Register set */
+#define BMA023_CTRL_REG0	0x0a
+#define BMA023_CTRL_REG1	0x0b
+#define BMA023_CTRL_REG2	0x14
+#define BMA023_CTRL_REG3	0x15
+
+#define BMA023_RANGE_MASK	GENMASK(4, 3) /* Range of accel values */
+#define BMA023_BW_MASK		GENMASK(2, 0) /* Accel bandwidth */
+#define BMA023_SLEEP		BIT(0)
+#define BMA023_INT_RESET_MASK	BIT(6)
+#define BMA023_NEW_DATA_INT	BIT(5) /* Intr every new accel data is ready */
+#define BMA023_RESET_VAL	BIT(1)
+
 #define BMA180_CHIP_ID		0x00 /* Need to distinguish BMA180 from other */
 #define BMA180_ACC_X_LSB	0x02 /* First of 6 registers of accel data */
 #define BMA180_TEMP		0x08
@@ -94,6 +109,7 @@ struct bma180_part_info {
 /* We have to write this value in reset register to do soft reset */
 #define BMA180_RESET_VAL	0xb6
 
+#define BMA023_ID_REG_VAL	0x02
 #define BMA180_ID_REG_VAL	0x03
 #define BMA250_ID_REG_VAL	0x03
 #define BMA254_ID_REG_VAL	0xfa /* 250 decimal */
@@ -156,6 +172,9 @@ enum bma180_chan {
 	TEMP
 };
 
+static int bma023_bw_table[] = { 25, 50, 100, 190, 375, 750, 1500 }; /* Hz */
+static int bma023_scale_table[] = { 2452, 4903, 9709, };
+
 static int bma180_bw_table[] = { 10, 20, 40, 75, 150, 300 }; /* Hz */
 static int bma180_scale_table[] = { 1275, 1863, 2452, 3727, 4903, 9709, 19417 };
 
@@ -319,7 +338,8 @@ static int bma180_set_pmode(struct bma180_data *data, bool mode)
 static int bma180_soft_reset(struct bma180_data *data)
 {
 	int ret = i2c_smbus_write_byte_data(data->client,
-		data->part_info->softreset_reg, BMA180_RESET_VAL);
+		data->part_info->softreset_reg,
+		data->part_info->softreset_val);
 
 	if (ret)
 		dev_err(&data->client->dev, "failed to reset the chip\n");
@@ -349,11 +369,28 @@ static int bma180_chip_init(struct bma180_data *data)
 	 */
 	msleep(20);
 
-	ret = bma180_set_new_data_intr_state(data, false);
+	return bma180_set_new_data_intr_state(data, false);
+}
+
+static int bma023_chip_config(struct bma180_data *data)
+{
+	int ret = bma180_chip_init(data);
+
 	if (ret)
-		return ret;
+		goto err;
+
+	ret = bma180_set_bw(data, 50); /* 50 Hz */
+	if (ret)
+		goto err;
+	ret = bma180_set_scale(data, 2452); /* 2 G */
+	if (ret)
+		goto err;
 
-	return bma180_set_pmode(data, false);
+	return 0;
+
+err:
+	dev_err(&data->client->dev, "failed to config the chip\n");
+	return ret;
 }
 
 static int bma180_chip_config(struct bma180_data *data)
@@ -362,6 +399,9 @@ static int bma180_chip_config(struct bma180_data *data)
 
 	if (ret)
 		goto err;
+	ret = bma180_set_pmode(data, false);
+	if (ret)
+		goto err;
 	ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_DIS_WAKE_UP, 1);
 	if (ret)
 		goto err;
@@ -391,6 +431,9 @@ static int bma25x_chip_config(struct bma180_data *data)
 
 	if (ret)
 		goto err;
+	ret = bma180_set_pmode(data, false);
+	if (ret)
+		goto err;
 	ret = bma180_set_bw(data, 16); /* 16 Hz */
 	if (ret)
 		goto err;
@@ -413,6 +456,17 @@ err:
 	return ret;
 }
 
+static void bma023_chip_disable(struct bma180_data *data)
+{
+	if (bma180_set_sleep_state(data, true))
+		goto err;
+
+	return;
+
+err:
+	dev_err(&data->client->dev, "failed to disable the chip\n");
+}
+
 static void bma180_chip_disable(struct bma180_data *data)
 {
 	if (bma180_set_new_data_intr_state(data, false))
@@ -512,8 +566,12 @@ static int bma180_read_raw(struct iio_dev *indio_dev,
 		iio_device_release_direct_mode(indio_dev);
 		if (ret < 0)
 			return ret;
-		*val = sign_extend32(ret >> chan->scan_type.shift,
-			chan->scan_type.realbits - 1);
+		if (chan->scan_type.sign == 's') {
+			*val = sign_extend32(ret >> chan->scan_type.shift,
+				chan->scan_type.realbits - 1);
+		} else {
+			*val = ret;
+		}
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
 		*val = data->bw;
@@ -531,7 +589,7 @@ static int bma180_read_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 		}
 	case IIO_CHAN_INFO_OFFSET:
-		*val = data->part_info->center_temp;
+		*val = data->part_info->temp_offset;
 		return IIO_VAL_INT;
 	default:
 		return -EINVAL;
@@ -609,6 +667,11 @@ static const struct iio_enum bma180_power_mode_enum = {
 	.set = bma180_set_power_mode,
 };
 
+static const struct iio_chan_spec_ext_info bma023_ext_info[] = {
+	IIO_MOUNT_MATRIX(IIO_SHARED_BY_DIR, bma180_accel_get_mount_matrix),
+	{ }
+};
+
 static const struct iio_chan_spec_ext_info bma180_ext_info[] = {
 	IIO_ENUM("power_mode", true, &bma180_power_mode_enum),
 	IIO_ENUM_AVAILABLE("power_mode", &bma180_power_mode_enum),
@@ -616,6 +679,35 @@ static const struct iio_chan_spec_ext_info bma180_ext_info[] = {
 	{ }
 };
 
+#define BMA023_ACC_CHANNEL(_axis, _bits) {				\
+	.type = IIO_ACCEL,						\
+	.modified = 1,							\
+	.channel2 = IIO_MOD_##_axis,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\
+		BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY),	\
+	.scan_index = AXIS_##_axis,					\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = _bits,					\
+		.storagebits = 16,					\
+		.shift = 16 - _bits,					\
+	},								\
+	.ext_info = bma023_ext_info,					\
+}
+
+#define BMA150_TEMP_CHANNEL {						\
+	.type = IIO_TEMP,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
+		BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_OFFSET),	\
+	.scan_index = TEMP,						\
+	.scan_type = {							\
+		.sign = 'u',						\
+		.realbits = 8,						\
+		.storagebits = 16,					\
+	},								\
+}
+
 #define BMA180_ACC_CHANNEL(_axis, _bits) {				\
 	.type = IIO_ACCEL,						\
 	.modified = 1,							\
@@ -645,6 +737,21 @@ static const struct iio_chan_spec_ext_info bma180_ext_info[] = {
 	},								\
 }
 
+static const struct iio_chan_spec bma023_channels[] = {
+	BMA023_ACC_CHANNEL(X, 10),
+	BMA023_ACC_CHANNEL(Y, 10),
+	BMA023_ACC_CHANNEL(Z, 10),
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static const struct iio_chan_spec bma150_channels[] = {
+	BMA023_ACC_CHANNEL(X, 10),
+	BMA023_ACC_CHANNEL(Y, 10),
+	BMA023_ACC_CHANNEL(Z, 10),
+	BMA150_TEMP_CHANNEL,
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
 static const struct iio_chan_spec bma180_channels[] = {
 	BMA180_ACC_CHANNEL(X, 14),
 	BMA180_ACC_CHANNEL(Y, 14),
@@ -670,6 +777,63 @@ static const struct iio_chan_spec bma254_channels[] = {
 };
 
 static const struct bma180_part_info bma180_part_info[] = {
+	[BMA023] = {
+		.chip_id = BMA023_ID_REG_VAL,
+		.channels = bma023_channels,
+		.num_channels = ARRAY_SIZE(bma023_channels),
+		.scale_table = bma023_scale_table,
+		.num_scales = ARRAY_SIZE(bma023_scale_table),
+		.bw_table = bma023_bw_table,
+		.num_bw = ARRAY_SIZE(bma023_bw_table),
+		/* No temperature channel */
+		.temp_offset = 0,
+		.int_reset_reg = BMA023_CTRL_REG0,
+		.int_reset_mask = BMA023_INT_RESET_MASK,
+		.sleep_reg = BMA023_CTRL_REG0,
+		.sleep_mask = BMA023_SLEEP,
+		.bw_reg = BMA023_CTRL_REG2,
+		.bw_mask = BMA023_BW_MASK,
+		.scale_reg = BMA023_CTRL_REG2,
+		.scale_mask = BMA023_RANGE_MASK,
+		/* No power mode on bma023 */
+		.power_reg = 0,
+		.power_mask = 0,
+		.lowpower_val = 0,
+		.int_enable_reg = BMA023_CTRL_REG3,
+		.int_enable_mask = BMA023_NEW_DATA_INT,
+		.softreset_reg = BMA023_CTRL_REG0,
+		.softreset_val = BMA023_RESET_VAL,
+		.chip_config = bma023_chip_config,
+		.chip_disable = bma023_chip_disable,
+	},
+	[BMA150] = {
+		.chip_id = BMA023_ID_REG_VAL,
+		.channels = bma150_channels,
+		.num_channels = ARRAY_SIZE(bma150_channels),
+		.scale_table = bma023_scale_table,
+		.num_scales = ARRAY_SIZE(bma023_scale_table),
+		.bw_table = bma023_bw_table,
+		.num_bw = ARRAY_SIZE(bma023_bw_table),
+		.temp_offset = -60, /* 0 LSB @ -30 degree C */
+		.int_reset_reg = BMA023_CTRL_REG0,
+		.int_reset_mask = BMA023_INT_RESET_MASK,
+		.sleep_reg = BMA023_CTRL_REG0,
+		.sleep_mask = BMA023_SLEEP,
+		.bw_reg = BMA023_CTRL_REG2,
+		.bw_mask = BMA023_BW_MASK,
+		.scale_reg = BMA023_CTRL_REG2,
+		.scale_mask = BMA023_RANGE_MASK,
+		/* No power mode on bma150 */
+		.power_reg = 0,
+		.power_mask = 0,
+		.lowpower_val = 0,
+		.int_enable_reg = BMA023_CTRL_REG3,
+		.int_enable_mask = BMA023_NEW_DATA_INT,
+		.softreset_reg = BMA023_CTRL_REG0,
+		.softreset_val = BMA023_RESET_VAL,
+		.chip_config = bma023_chip_config,
+		.chip_disable = bma023_chip_disable,
+	},
 	[BMA180] = {
 		.chip_id = BMA180_ID_REG_VAL,
 		.channels = bma180_channels,
@@ -678,7 +842,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.num_scales = ARRAY_SIZE(bma180_scale_table),
 		.bw_table = bma180_bw_table,
 		.num_bw = ARRAY_SIZE(bma180_bw_table),
-		.center_temp = 48, /* 0 LSB @ 24 degree C */
+		.temp_offset = 48, /* 0 LSB @ 24 degree C */
 		.int_reset_reg = BMA180_CTRL_REG0,
 		.int_reset_mask = BMA180_RESET_INT,
 		.sleep_reg = BMA180_CTRL_REG0,
@@ -693,6 +857,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.int_enable_reg = BMA180_CTRL_REG3,
 		.int_enable_mask = BMA180_NEW_DATA_INT,
 		.softreset_reg = BMA180_RESET,
+		.softreset_val = BMA180_RESET_VAL,
 		.chip_config = bma180_chip_config,
 		.chip_disable = bma180_chip_disable,
 	},
@@ -704,7 +869,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.num_scales = ARRAY_SIZE(bma25x_scale_table),
 		.bw_table = bma25x_bw_table,
 		.num_bw = ARRAY_SIZE(bma25x_bw_table),
-		.center_temp = 48, /* 0 LSB @ 24 degree C */
+		.temp_offset = 48, /* 0 LSB @ 24 degree C */
 		.int_reset_reg = BMA250_INT_RESET_REG,
 		.int_reset_mask = BMA250_INT_RESET_MASK,
 		.sleep_reg = BMA250_POWER_REG,
@@ -721,6 +886,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.int_map_reg = BMA250_INT_MAP_REG,
 		.int_enable_dataready_int1_mask = BMA250_INT1_DATA_MASK,
 		.softreset_reg = BMA250_RESET_REG,
+		.softreset_val = BMA180_RESET_VAL,
 		.chip_config = bma25x_chip_config,
 		.chip_disable = bma25x_chip_disable,
 	},
@@ -732,7 +898,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.num_scales = ARRAY_SIZE(bma25x_scale_table),
 		.bw_table = bma25x_bw_table,
 		.num_bw = ARRAY_SIZE(bma25x_bw_table),
-		.center_temp = 46, /* 0 LSB @ 23 degree C */
+		.temp_offset = 46, /* 0 LSB @ 23 degree C */
 		.int_reset_reg = BMA254_INT_RESET_REG,
 		.int_reset_mask = BMA254_INT_RESET_MASK,
 		.sleep_reg = BMA254_POWER_REG,
@@ -749,6 +915,7 @@ static const struct bma180_part_info bma180_part_info[] = {
 		.int_map_reg = BMA254_INT_MAP_REG,
 		.int_enable_dataready_int1_mask = BMA254_INT1_DATA_MASK,
 		.softreset_reg = BMA254_RESET_REG,
+		.softreset_val = BMA180_RESET_VAL,
 		.chip_config = bma25x_chip_config,
 		.chip_disable = bma25x_chip_disable,
 	},
@@ -990,9 +1157,12 @@ static SIMPLE_DEV_PM_OPS(bma180_pm_ops, bma180_suspend, bma180_resume);
 #endif
 
 static const struct i2c_device_id bma180_ids[] = {
+	{ "bma023", BMA023 },
+	{ "bma150", BMA150 },
 	{ "bma180", BMA180 },
 	{ "bma250", BMA250 },
 	{ "bma254", BMA254 },
+	{ "smb380", BMA150 },
 	{ }
 };
 
@@ -1000,6 +1170,14 @@ MODULE_DEVICE_TABLE(i2c, bma180_ids);
 
 static const struct of_device_id bma180_of_match[] = {
 	{
+		.compatible = "bosch,bma023",
+		.data = (void *)BMA023
+	},
+	{
+		.compatible = "bosch,bma150",
+		.data = (void *)BMA150
+	},
+	{
 		.compatible = "bosch,bma180",
 		.data = (void *)BMA180
 	},
@@ -1011,6 +1189,10 @@ static const struct of_device_id bma180_of_match[] = {
 		.compatible = "bosch,bma254",
 		.data = (void *)BMA254
 	},
+	{
+		.compatible = "bosch,smb380",
+		.data = (void *)BMA150
+	},
 	{ }
 };
 MODULE_DEVICE_TABLE(of, bma180_of_match);
@@ -1030,5 +1212,5 @@ module_i2c_driver(bma180_driver);
 
 MODULE_AUTHOR("Kravchenko Oleksandr <x0199363@ti.com>");
 MODULE_AUTHOR("Texas Instruments, Inc.");
-MODULE_DESCRIPTION("Bosch BMA180/BMA25x triaxial acceleration sensor");
+MODULE_DESCRIPTION("Bosch BMA023/BMA1x0/BMA25x triaxial acceleration sensor");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/accel/dmard06.c b/drivers/iio/accel/dmard06.c
index 2bf210fa4ba6..ef89bded7390 100644
--- a/drivers/iio/accel/dmard06.c
+++ b/drivers/iio/accel/dmard06.c
@@ -6,6 +6,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/i2c.h>
 #include <linux/iio/iio.h>
 
@@ -226,7 +227,7 @@ static struct i2c_driver dmard06_driver = {
 	.id_table = dmard06_id,
 	.driver = {
 		.name = DMARD06_DRV_NAME,
-		.of_match_table = of_match_ptr(dmard06_of_match),
+		.of_match_table = dmard06_of_match,
 		.pm = DMARD06_PM_OPS,
 	},
 };
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 0d9e2def2b25..0ec0533448bc 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 enum accel_3d_channel {
@@ -391,18 +389,13 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&accel_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&accel_state->common_attributes);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -426,9 +419,7 @@ static int hid_accel_3d_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&accel_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &accel_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -443,8 +434,7 @@ static int hid_accel_3d_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, hsdev->usage);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&accel_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &accel_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/accel/kxsd9-i2c.c b/drivers/iio/accel/kxsd9-i2c.c
index 38411e1c155b..b580d605f848 100644
--- a/drivers/iio/accel/kxsd9-i2c.c
+++ b/drivers/iio/accel/kxsd9-i2c.c
@@ -2,6 +2,7 @@
 #include <linux/device.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
 #include <linux/delay.h>
@@ -21,8 +22,8 @@ static int kxsd9_i2c_probe(struct i2c_client *i2c,
 
 	regmap = devm_regmap_init_i2c(i2c, &config);
 	if (IS_ERR(regmap)) {
-		dev_err(&i2c->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&i2c->dev, "Failed to register i2c regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
@@ -36,15 +37,11 @@ static int kxsd9_i2c_remove(struct i2c_client *client)
 	return kxsd9_common_remove(&client->dev);
 }
 
-#ifdef CONFIG_OF
 static const struct of_device_id kxsd9_of_match[] = {
 	{ .compatible = "kionix,kxsd9", },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, kxsd9_of_match);
-#else
-#define kxsd9_of_match NULL
-#endif
 
 static const struct i2c_device_id kxsd9_i2c_id[] = {
 	{"kxsd9", 0},
@@ -55,7 +52,7 @@ MODULE_DEVICE_TABLE(i2c, kxsd9_i2c_id);
 static struct i2c_driver kxsd9_i2c_driver = {
 	.driver = {
 		.name	= "kxsd9",
-		.of_match_table = of_match_ptr(kxsd9_of_match),
+		.of_match_table = kxsd9_of_match,
 		.pm = &kxsd9_dev_pm_ops,
 	},
 	.probe		= kxsd9_i2c_probe,
diff --git a/drivers/iio/accel/mxc4005.c b/drivers/iio/accel/mxc4005.c
index 3d5bea651923..9d07642c0de1 100644
--- a/drivers/iio/accel/mxc4005.c
+++ b/drivers/iio/accel/mxc4005.c
@@ -135,7 +135,7 @@ static int mxc4005_read_xyz(struct mxc4005_data *data)
 	int ret;
 
 	ret = regmap_bulk_read(data->regmap, MXC4005_REG_XOUT_UPPER,
-			       (u8 *) data->buffer, sizeof(data->buffer));
+			       data->buffer, sizeof(data->buffer));
 	if (ret < 0) {
 		dev_err(data->dev, "failed to read axes\n");
 		return ret;
@@ -150,7 +150,7 @@ static int mxc4005_read_axis(struct mxc4005_data *data,
 	__be16 reg;
 	int ret;
 
-	ret = regmap_bulk_read(data->regmap, addr, (u8 *) &reg, sizeof(reg));
+	ret = regmap_bulk_read(data->regmap, addr, &reg, sizeof(reg));
 	if (ret < 0) {
 		dev_err(data->dev, "failed to read reg %02x\n", addr);
 		return ret;
diff --git a/drivers/iio/accel/st_accel.h b/drivers/iio/accel/st_accel.h
index 5b13e293cade..5d356288e001 100644
--- a/drivers/iio/accel/st_accel.h
+++ b/drivers/iio/accel/st_accel.h
@@ -35,6 +35,7 @@ enum st_accel_type {
 	LIS2DW12,
 	LIS3DHH,
 	LIS2DE12,
+	LIS2HH12,
 	ST_ACCEL_MAX,
 };
 
@@ -59,6 +60,7 @@ enum st_accel_type {
 #define LIS3DHH_ACCEL_DEV_NAME		"lis3dhh"
 #define LIS3DE_ACCEL_DEV_NAME		"lis3de"
 #define LIS2DE12_ACCEL_DEV_NAME		"lis2de12"
+#define LIS2HH12_ACCEL_DEV_NAME		"lis2hh12"
 
 /**
 * struct st_sensors_platform_data - default accel platform data
diff --git a/drivers/iio/accel/st_accel_buffer.c b/drivers/iio/accel/st_accel_buffer.c
index 9f2b40474b8e..b5c814ef1637 100644
--- a/drivers/iio/accel/st_accel_buffer.c
+++ b/drivers/iio/accel/st_accel_buffer.c
@@ -37,8 +37,7 @@ static int st_accel_buffer_postenable(struct iio_dev *indio_dev)
 	if (err < 0)
 		return err;
 
-	err = st_sensors_set_axis_enable(indio_dev,
-					 (u8)indio_dev->active_scan_mask[0]);
+	err = st_sensors_set_axis_enable(indio_dev, indio_dev->active_scan_mask[0]);
 	if (err < 0)
 		goto st_accel_buffer_predisable;
 
diff --git a/drivers/iio/accel/st_accel_core.c b/drivers/iio/accel/st_accel_core.c
index 7320275c7e56..43c50167d220 100644
--- a/drivers/iio/accel/st_accel_core.c
+++ b/drivers/iio/accel/st_accel_core.c
@@ -904,6 +904,83 @@ static const struct st_sensor_settings st_accel_sensors_settings[] = {
 		.multi_read_bit = true,
 		.bootime = 2,
 	},
+	{
+		.wai = 0x41,
+		.wai_addr = ST_SENSORS_DEFAULT_WAI_ADDRESS,
+		.sensors_supported = {
+			[0] = LIS2HH12_ACCEL_DEV_NAME,
+		},
+		.ch = (struct iio_chan_spec *)st_accel_16bit_channels,
+		.odr = {
+			.addr = 0x20,
+			.mask = 0x70,
+			.odr_avl = {
+				{ .hz = 10, .value = 0x01, },
+				{ .hz = 50, .value = 0x02, },
+				{ .hz = 100, .value = 0x03, },
+				{ .hz = 200, .value = 0x04, },
+				{ .hz = 400, .value = 0x05, },
+				{ .hz = 800, .value = 0x06, },
+			},
+		},
+		.pw = {
+			.addr = 0x20,
+			.mask = 0x70,
+			.value_off = ST_SENSORS_DEFAULT_POWER_OFF_VALUE,
+		},
+		.enable_axis = {
+			.addr = ST_SENSORS_DEFAULT_AXIS_ADDR,
+			.mask = ST_SENSORS_DEFAULT_AXIS_MASK,
+		},
+		.fs = {
+			.addr = 0x23,
+			.mask = 0x30,
+			.fs_avl = {
+				[0] = {
+					.num = ST_ACCEL_FS_AVL_2G,
+					.value = 0x00,
+					.gain = IIO_G_TO_M_S_2(61),
+				},
+				[1] = {
+					.num = ST_ACCEL_FS_AVL_4G,
+					.value = 0x02,
+					.gain = IIO_G_TO_M_S_2(122),
+				},
+				[2] = {
+					.num = ST_ACCEL_FS_AVL_8G,
+					.value = 0x03,
+					.gain = IIO_G_TO_M_S_2(244),
+				},
+			},
+		},
+		.bdu = {
+			.addr = 0x20,
+			.mask = 0x08,
+		},
+		.drdy_irq = {
+			.int1 = {
+				.addr = 0x22,
+				.mask = 0x01,
+			},
+			.int2 = {
+				.addr = 0x25,
+				.mask = 0x01,
+			},
+			.addr_ihl = 0x24,
+			.mask_ihl = 0x02,
+			.stat_drdy = {
+				.addr = ST_SENSORS_DEFAULT_STAT_ADDR,
+				.mask = 0x07,
+			},
+		},
+		.sim = {
+			.addr = 0x23,
+			.value = BIT(0),
+		},
+		.multi_read_bit = true,
+		.bootime = 2,
+	},
+
 };
 
 static int st_accel_read_raw(struct iio_dev *indio_dev,
@@ -1170,8 +1247,7 @@ EXPORT_SYMBOL(st_accel_get_settings);
 int st_accel_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *adata = iio_priv(indio_dev);
-	struct st_sensors_platform_data *pdata =
-		(struct st_sensors_platform_data *)adata->dev->platform_data;
+	struct st_sensors_platform_data *pdata = dev_get_platdata(adata->dev);
 	struct iio_chan_spec *channels;
 	size_t channels_size;
 	int err;
@@ -1204,8 +1280,7 @@ int st_accel_common_probe(struct iio_dev *indio_dev)
 			 "failed to apply ACPI orientation data: %d\n", err);
 
 	indio_dev->channels = channels;
-	adata->current_fullscale = (struct st_sensor_fullscale_avl *)
-					&adata->sensor_settings->fs.fs_avl[0];
+	adata->current_fullscale = &adata->sensor_settings->fs.fs_avl[0];
 	adata->odr = adata->sensor_settings->odr.odr_avl[0].hz;
 
 	if (!pdata)
diff --git a/drivers/iio/accel/st_accel_i2c.c b/drivers/iio/accel/st_accel_i2c.c
index 6b283be26ebc..360e16f2cadb 100644
--- a/drivers/iio/accel/st_accel_i2c.c
+++ b/drivers/iio/accel/st_accel_i2c.c
@@ -104,6 +104,10 @@ static const struct of_device_id st_accel_of_match[] = {
 		.compatible = "st,lis2de12",
 		.data = LIS2DE12_ACCEL_DEV_NAME,
 	},
+	{
+		.compatible = "st,lis2hh12",
+		.data = LIS2HH12_ACCEL_DEV_NAME,
+	},
 	{},
 };
 MODULE_DEVICE_TABLE(of, st_accel_of_match);
@@ -138,6 +142,7 @@ static const struct i2c_device_id st_accel_id_table[] = {
 	{ LIS2DW12_ACCEL_DEV_NAME },
 	{ LIS3DE_ACCEL_DEV_NAME },
 	{ LIS2DE12_ACCEL_DEV_NAME },
+	{ LIS2HH12_ACCEL_DEV_NAME },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, st_accel_id_table);
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index a864ede98114..ff3569635ce0 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -246,6 +246,41 @@ config AD799X
 	  To compile this driver as a module, choose M here: the module will be
 	  called ad799x.
 
+config AD9467
+	tristate "Analog Devices AD9467 High Speed ADC driver"
+	depends on SPI
+	select ADI_AXI_ADC
+	help
+	  Say yes here to build support for Analog Devices:
+	  * AD9467 16-Bit, 200 MSPS/250 MSPS Analog-to-Digital Converter
+
+	  The driver requires the assistance of the AXI ADC IP core to operate,
+	  since SPI is used for configuration only, while data has to be
+	  streamed into memory via DMA.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ad9467.
+
+config ADI_AXI_ADC
+	tristate "Analog Devices Generic AXI ADC IP core driver"
+	select IIO_BUFFER
+	select IIO_BUFFER_HW_CONSUMER
+	select IIO_BUFFER_DMAENGINE
+	help
+	  Say yes here to build support for Analog Devices Generic
+	  AXI ADC IP core. The IP core is used for interfacing with
+	  analog-to-digital (ADC) converters that require either a high-speed
+	  serial interface (JESD204B/C) or a source synchronous parallel
+	  interface (LVDS/CMOS).
+	  Typically (for such devices) SPI will be used for configuration only,
+	  while this IP core handles the streaming of data into memory via DMA.
+
+	  Link: https://wiki.analog.com/resources/fpga/docs/axi_adc_ip
+	  If unsure, say N (but it's safe to say "Y").
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called adi-axi-adc.
+
 config ASPEED_ADC
 	tristate "Aspeed ADC"
 	depends on ARCH_ASPEED || COMPILE_TEST
@@ -595,6 +630,16 @@ config MAX1118
 	  To compile this driver as a module, choose M here: the module will be
 	  called max1118.
 
+config MAX1241
+	tristate "Maxim max1241 ADC driver"
+	depends on SPI_MASTER
+	help
+	  Say yes here to build support for Maxim max1241 12-bit, single-channel
+	  ADC.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called max1241.
+
 config MAX1363
 	tristate "Maxim max1363 ADC driver"
 	depends on I2C
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index 78a1963a14f3..90f94ada7b30 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -26,6 +26,8 @@ obj-$(CONFIG_AD7793) += ad7793.o
 obj-$(CONFIG_AD7887) += ad7887.o
 obj-$(CONFIG_AD7949) += ad7949.o
 obj-$(CONFIG_AD799X) += ad799x.o
+obj-$(CONFIG_AD9467) += ad9467.o
+obj-$(CONFIG_ADI_AXI_ADC) += adi-axi-adc.o
 obj-$(CONFIG_ASPEED_ADC) += aspeed_adc.o
 obj-$(CONFIG_AT91_ADC) += at91_adc.o
 obj-$(CONFIG_AT91_SAMA5D2_ADC) += at91-sama5d2_adc.o
@@ -57,6 +59,7 @@ obj-$(CONFIG_LTC2497) += ltc2497.o ltc2497-core.o
 obj-$(CONFIG_MAX1027) += max1027.o
 obj-$(CONFIG_MAX11100) += max11100.o
 obj-$(CONFIG_MAX1118) += max1118.o
+obj-$(CONFIG_MAX1241) += max1241.o
 obj-$(CONFIG_MAX1363) += max1363.o
 obj-$(CONFIG_MAX9611) += max9611.o
 obj-$(CONFIG_MCP320X) += mcp320x.o
diff --git a/drivers/iio/adc/ad7476.c b/drivers/iio/adc/ad7476.c
index 76747488044b..4e816d714ad2 100644
--- a/drivers/iio/adc/ad7476.c
+++ b/drivers/iio/adc/ad7476.c
@@ -12,9 +12,11 @@
 #include <linux/sysfs.h>
 #include <linux/spi/spi.h>
 #include <linux/regulator/consumer.h>
+#include <linux/gpio/consumer.h>
 #include <linux/err.h>
 #include <linux/module.h>
 #include <linux/bitops.h>
+#include <linux/delay.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -27,6 +29,8 @@ struct ad7476_state;
 struct ad7476_chip_info {
 	unsigned int			int_vref_uv;
 	struct iio_chan_spec		channel[2];
+	/* channels used when convst gpio is defined */
+	struct iio_chan_spec		convst_channel[2];
 	void (*reset)(struct ad7476_state *);
 };
 
@@ -34,6 +38,7 @@ struct ad7476_state {
 	struct spi_device		*spi;
 	const struct ad7476_chip_info	*chip_info;
 	struct regulator		*reg;
+	struct gpio_desc		*convst_gpio;
 	struct spi_transfer		xfer;
 	struct spi_message		msg;
 	/*
@@ -64,6 +69,17 @@ enum ad7476_supported_device_ids {
 	ID_ADS7868,
 };
 
+static void ad7091_convst(struct ad7476_state *st)
+{
+	if (!st->convst_gpio)
+		return;
+
+	gpiod_set_value(st->convst_gpio, 0);
+	udelay(1); /* CONVST pulse width: 10 ns min */
+	gpiod_set_value(st->convst_gpio, 1);
+	udelay(1); /* Conversion time: 650 ns max */
+}
+
 static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 {
 	struct iio_poll_func *pf = p;
@@ -71,6 +87,8 @@ static irqreturn_t ad7476_trigger_handler(int irq, void  *p)
 	struct ad7476_state *st = iio_priv(indio_dev);
 	int b_sent;
 
+	ad7091_convst(st);
+
 	b_sent = spi_sync(st->spi, &st->msg);
 	if (b_sent < 0)
 		goto done;
@@ -93,6 +111,8 @@ static int ad7476_scan_direct(struct ad7476_state *st)
 {
 	int ret;
 
+	ad7091_convst(st);
+
 	ret = spi_sync(st->spi, &st->msg);
 	if (ret)
 		return ret;
@@ -160,6 +180,8 @@ static int ad7476_read_raw(struct iio_dev *indio_dev,
 #define AD7940_CHAN(bits) _AD7476_CHAN((bits), 15 - (bits), \
 		BIT(IIO_CHAN_INFO_RAW))
 #define AD7091R_CHAN(bits) _AD7476_CHAN((bits), 16 - (bits), 0)
+#define AD7091R_CONVST_CHAN(bits) _AD7476_CHAN((bits), 16 - (bits), \
+		BIT(IIO_CHAN_INFO_RAW))
 #define ADS786X_CHAN(bits) _AD7476_CHAN((bits), 12 - (bits), \
 		BIT(IIO_CHAN_INFO_RAW))
 
@@ -167,6 +189,8 @@ static const struct ad7476_chip_info ad7476_chip_info_tbl[] = {
 	[ID_AD7091R] = {
 		.channel[0] = AD7091R_CHAN(12),
 		.channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
+		.convst_channel[0] = AD7091R_CONVST_CHAN(12),
+		.convst_channel[1] = IIO_CHAN_SOFT_TIMESTAMP(1),
 		.reset = ad7091_reset,
 	},
 	[ID_AD7276] = {
@@ -232,6 +256,13 @@ static const struct iio_info ad7476_info = {
 	.read_raw = &ad7476_read_raw,
 };
 
+static void ad7476_reg_disable(void *data)
+{
+	struct ad7476_state *st = data;
+
+	regulator_disable(st->reg);
+}
+
 static int ad7476_probe(struct spi_device *spi)
 {
 	struct ad7476_state *st;
@@ -254,6 +285,17 @@ static int ad7476_probe(struct spi_device *spi)
 	if (ret)
 		return ret;
 
+	ret = devm_add_action_or_reset(&spi->dev, ad7476_reg_disable,
+				       st);
+	if (ret)
+		return ret;
+
+	st->convst_gpio = devm_gpiod_get_optional(&spi->dev,
+						  "adi,conversion-start",
+						  GPIOD_OUT_LOW);
+	if (IS_ERR(st->convst_gpio))
+		return PTR_ERR(st->convst_gpio);
+
 	spi_set_drvdata(spi, indio_dev);
 
 	st->spi = spi;
@@ -266,6 +308,9 @@ static int ad7476_probe(struct spi_device *spi)
 	indio_dev->channels = st->chip_info->channel;
 	indio_dev->num_channels = 2;
 	indio_dev->info = &ad7476_info;
+
+	if (st->convst_gpio)
+		indio_dev->channels = st->chip_info->convst_channel;
 	/* Setup default message */
 
 	st->xfer.rx_buf = &st->data;
@@ -295,19 +340,8 @@ error_disable_reg:
 	return ret;
 }
 
-static int ad7476_remove(struct spi_device *spi)
-{
-	struct iio_dev *indio_dev = spi_get_drvdata(spi);
-	struct ad7476_state *st = iio_priv(indio_dev);
-
-	iio_device_unregister(indio_dev);
-	iio_triggered_buffer_cleanup(indio_dev);
-	regulator_disable(st->reg);
-
-	return 0;
-}
-
 static const struct spi_device_id ad7476_id[] = {
+	{"ad7091", ID_AD7091R},
 	{"ad7091r", ID_AD7091R},
 	{"ad7273", ID_AD7277},
 	{"ad7274", ID_AD7276},
@@ -343,7 +377,6 @@ static struct spi_driver ad7476_driver = {
 		.name	= "ad7476",
 	},
 	.probe		= ad7476_probe,
-	.remove		= ad7476_remove,
 	.id_table	= ad7476_id,
 };
 module_spi_driver(ad7476_driver);
diff --git a/drivers/iio/adc/ad7780.c b/drivers/iio/adc/ad7780.c
index 291c1a898129..f47606ebbbbe 100644
--- a/drivers/iio/adc/ad7780.c
+++ b/drivers/iio/adc/ad7780.c
@@ -206,10 +206,29 @@ static const struct ad_sigma_delta_info ad7780_sigma_delta_info = {
 	.irq_flags = IRQF_TRIGGER_LOW,
 };
 
-#define AD7780_CHANNEL(bits, wordsize) \
-	AD_SD_CHANNEL(1, 0, 0, bits, 32, (wordsize) - (bits))
-#define AD7170_CHANNEL(bits, wordsize) \
-	AD_SD_CHANNEL_NO_SAMP_FREQ(1, 0, 0, bits, 32, (wordsize) - (bits))
+#define _AD7780_CHANNEL(_bits, _wordsize, _mask_all)		\
+{								\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = 0,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+		BIT(IIO_CHAN_INFO_OFFSET),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.info_mask_shared_by_all = _mask_all,			\
+	.scan_index = 1,					\
+	.scan_type = {						\
+		.sign = 'u',					\
+		.realbits = (_bits),				\
+		.storagebits = 32,				\
+		.shift = (_wordsize) - (_bits),			\
+		.endianness = IIO_BE,				\
+	},							\
+}
+
+#define AD7780_CHANNEL(_bits, _wordsize)	\
+	_AD7780_CHANNEL(_bits, _wordsize, BIT(IIO_CHAN_INFO_SAMP_FREQ))
+#define AD7170_CHANNEL(_bits, _wordsize)	\
+	_AD7780_CHANNEL(_bits, _wordsize, 0)
 
 static const struct ad7780_chip_info ad7780_chip_info_tbl[] = {
 	[ID_AD7170] = {
diff --git a/drivers/iio/adc/ad7791.c b/drivers/iio/adc/ad7791.c
index abb239392631..48432b6f6002 100644
--- a/drivers/iio/adc/ad7791.c
+++ b/drivers/iio/adc/ad7791.c
@@ -64,25 +64,73 @@
 #define AD7791_MODE_SEL_MASK		(0x3 << 6)
 #define AD7791_MODE_SEL(x)		((x) << 6)
 
+#define __AD7991_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+	_storagebits, _shift, _extend_name, _type, _mask_all) \
+	{ \
+		.type = (_type), \
+		.differential = (_channel2 == -1 ? 0 : 1), \
+		.indexed = 1, \
+		.channel = (_channel1), \
+		.channel2 = (_channel2), \
+		.address = (_address), \
+		.extend_name = (_extend_name), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_OFFSET), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+		.info_mask_shared_by_all = _mask_all, \
+		.scan_index = (_si), \
+		.scan_type = { \
+			.sign = 'u', \
+			.realbits = (_bits), \
+			.storagebits = (_storagebits), \
+			.shift = (_shift), \
+			.endianness = IIO_BE, \
+		}, \
+	}
+
+#define AD7991_SHORTED_CHANNEL(_si, _channel, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7991_CHANNEL(_si, _channel, _channel, _address, _bits, \
+		_storagebits, _shift, "shorted", IIO_VOLTAGE, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7991_CHANNEL(_si, _channel, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7991_CHANNEL(_si, _channel, -1, _address, _bits, \
+		_storagebits, _shift, NULL, IIO_VOLTAGE, \
+		 BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7991_DIFF_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7991_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+		_storagebits, _shift, NULL, IIO_VOLTAGE, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7991_SUPPLY_CHANNEL(_si, _channel, _address, _bits, _storagebits, \
+	_shift) \
+	__AD7991_CHANNEL(_si, _channel, -1, _address, _bits, \
+		_storagebits, _shift, "supply", IIO_VOLTAGE, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
 #define DECLARE_AD7787_CHANNELS(name, bits, storagebits) \
 const struct iio_chan_spec name[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
+	AD7991_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
 		(bits), (storagebits), 0), \
-	AD_SD_CHANNEL(1, 1, AD7791_CH_AIN2, (bits), (storagebits), 0), \
-	AD_SD_SHORTED_CHANNEL(2, 0, AD7791_CH_AIN1N_AIN1N, \
+	AD7991_CHANNEL(1, 1, AD7791_CH_AIN2, (bits), (storagebits), 0), \
+	AD7991_SHORTED_CHANNEL(2, 0, AD7791_CH_AIN1N_AIN1N, \
 		(bits), (storagebits), 0), \
-	AD_SD_SUPPLY_CHANNEL(3, 2, AD7791_CH_AVDD_MONITOR,  \
+	AD7991_SUPPLY_CHANNEL(3, 2, AD7791_CH_AVDD_MONITOR,  \
 		(bits), (storagebits), 0), \
 	IIO_CHAN_SOFT_TIMESTAMP(4), \
 }
 
 #define DECLARE_AD7791_CHANNELS(name, bits, storagebits) \
 const struct iio_chan_spec name[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
+	AD7991_DIFF_CHANNEL(0, 0, 0, AD7791_CH_AIN1P_AIN1N, \
 		(bits), (storagebits), 0), \
-	AD_SD_SHORTED_CHANNEL(1, 0, AD7791_CH_AIN1N_AIN1N, \
+	AD7991_SHORTED_CHANNEL(1, 0, AD7791_CH_AIN1N_AIN1N, \
 		(bits), (storagebits), 0), \
-	AD_SD_SUPPLY_CHANNEL(2, 1, AD7791_CH_AVDD_MONITOR, \
+	AD7991_SUPPLY_CHANNEL(2, 1, AD7791_CH_AVDD_MONITOR, \
 		(bits), (storagebits), 0), \
 	IIO_CHAN_SOFT_TIMESTAMP(3), \
 }
@@ -444,5 +492,5 @@ static struct spi_driver ad7791_driver = {
 module_spi_driver(ad7791_driver);
 
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
-MODULE_DESCRIPTION("Analog Device AD7787/AD7788/AD7789/AD7790/AD7791 ADC driver");
+MODULE_DESCRIPTION("Analog Devices AD7787/AD7788/AD7789/AD7790/AD7791 ADC driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ad7793.c b/drivers/iio/adc/ad7793.c
index e5691e330323..808485f42415 100644
--- a/drivers/iio/adc/ad7793.c
+++ b/drivers/iio/adc/ad7793.c
@@ -354,29 +354,28 @@ static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
 static IIO_CONST_ATTR_NAMED(sampling_frequency_available_ad7797,
 	sampling_frequency_available, "123 62 50 33 17 16 12 10 8 6 4");
 
-static ssize_t ad7793_show_scale_available(struct device *dev,
-			struct device_attribute *attr, char *buf)
+static int ad7793_read_avail(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     const int **vals, int *type, int *length,
+			     long mask)
 {
-	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	struct ad7793_state *st = iio_priv(indio_dev);
-	int i, len = 0;
 
-	for (i = 0; i < ARRAY_SIZE(st->scale_avail); i++)
-		len += sprintf(buf + len, "%d.%09u ", st->scale_avail[i][0],
-			       st->scale_avail[i][1]);
-
-	len += sprintf(buf + len, "\n");
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		*vals = (int *)st->scale_avail;
+		*type = IIO_VAL_INT_PLUS_NANO;
+		/* Values are stored in a 2D matrix  */
+		*length = ARRAY_SIZE(st->scale_avail) * 2;
 
-	return len;
+		return IIO_AVAIL_LIST;
+	default:
+		return -EINVAL;
+	}
 }
 
-static IIO_DEVICE_ATTR_NAMED(in_m_in_scale_available,
-		in_voltage-voltage_scale_available, S_IRUGO,
-		ad7793_show_scale_available, NULL, 0);
-
 static struct attribute *ad7793_attributes[] = {
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
-	&iio_dev_attr_in_m_in_scale_available.dev_attr.attr,
 	NULL
 };
 
@@ -534,6 +533,7 @@ static const struct iio_info ad7793_info = {
 	.read_raw = &ad7793_read_raw,
 	.write_raw = &ad7793_write_raw,
 	.write_raw_get_fmt = &ad7793_write_raw_get_fmt,
+	.read_avail = ad7793_read_avail,
 	.attrs = &ad7793_attribute_group,
 	.validate_trigger = ad_sd_validate_trigger,
 };
@@ -546,47 +546,113 @@ static const struct iio_info ad7797_info = {
 	.validate_trigger = ad_sd_validate_trigger,
 };
 
+#define __AD7793_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+	_storagebits, _shift, _extend_name, _type, _mask_type_av, _mask_all) \
+	{ \
+		.type = (_type), \
+		.differential = (_channel2 == -1 ? 0 : 1), \
+		.indexed = 1, \
+		.channel = (_channel1), \
+		.channel2 = (_channel2), \
+		.address = (_address), \
+		.extend_name = (_extend_name), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_OFFSET), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+		.info_mask_shared_by_type_available = (_mask_type_av), \
+		.info_mask_shared_by_all = _mask_all, \
+		.scan_index = (_si), \
+		.scan_type = { \
+			.sign = 'u', \
+			.realbits = (_bits), \
+			.storagebits = (_storagebits), \
+			.shift = (_shift), \
+			.endianness = IIO_BE, \
+		}, \
+	}
+
+#define AD7793_DIFF_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7793_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+		_storagebits, _shift, NULL, IIO_VOLTAGE, \
+		BIT(IIO_CHAN_INFO_SCALE), \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7793_SHORTED_CHANNEL(_si, _channel, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7793_CHANNEL(_si, _channel, _channel, _address, _bits, \
+		_storagebits, _shift, "shorted", IIO_VOLTAGE, \
+		BIT(IIO_CHAN_INFO_SCALE), \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7793_TEMP_CHANNEL(_si, _address, _bits, _storagebits, _shift) \
+	__AD7793_CHANNEL(_si, 0, -1, _address, _bits, \
+		_storagebits, _shift, NULL, IIO_TEMP, \
+		0, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7793_SUPPLY_CHANNEL(_si, _channel, _address, _bits, _storagebits, \
+	_shift) \
+	__AD7793_CHANNEL(_si, _channel, -1, _address, _bits, \
+		_storagebits, _shift, "supply", IIO_VOLTAGE, \
+		0, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7797_DIFF_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7793_CHANNEL(_si, _channel1, _channel2, _address, _bits, \
+		_storagebits, _shift, NULL, IIO_VOLTAGE, \
+		0, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
+#define AD7797_SHORTED_CHANNEL(_si, _channel, _address, _bits, \
+	_storagebits, _shift) \
+	__AD7793_CHANNEL(_si, _channel, _channel, _address, _bits, \
+		_storagebits, _shift, "shorted", IIO_VOLTAGE, \
+		0, \
+		BIT(IIO_CHAN_INFO_SAMP_FREQ))
+
 #define DECLARE_AD7793_CHANNELS(_name, _b, _sb, _s) \
 const struct iio_chan_spec _name##_channels[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), (_s)), \
-	AD_SD_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), (_s)), \
-	AD_SD_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), (_s)), \
-	AD_SD_SHORTED_CHANNEL(3, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), (_s)), \
-	AD_SD_TEMP_CHANNEL(4, AD7793_CH_TEMP, (_b), (_sb), (_s)), \
-	AD_SD_SUPPLY_CHANNEL(5, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), (_s)), \
+	AD7793_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), (_s)), \
+	AD7793_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), (_s)), \
+	AD7793_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), (_s)), \
+	AD7793_SHORTED_CHANNEL(3, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), (_s)), \
+	AD7793_TEMP_CHANNEL(4, AD7793_CH_TEMP, (_b), (_sb), (_s)), \
+	AD7793_SUPPLY_CHANNEL(5, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), (_s)), \
 	IIO_CHAN_SOFT_TIMESTAMP(6), \
 }
 
 #define DECLARE_AD7795_CHANNELS(_name, _b, _sb) \
 const struct iio_chan_spec _name##_channels[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(3, 3, 3, AD7795_CH_AIN4P_AIN4M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(4, 4, 4, AD7795_CH_AIN5P_AIN5M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(5, 5, 5, AD7795_CH_AIN6P_AIN6M, (_b), (_sb), 0), \
-	AD_SD_SHORTED_CHANNEL(6, 0, AD7795_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
-	AD_SD_TEMP_CHANNEL(7, AD7793_CH_TEMP, (_b), (_sb), 0), \
-	AD_SD_SUPPLY_CHANNEL(8, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(3, 3, 3, AD7795_CH_AIN4P_AIN4M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(4, 4, 4, AD7795_CH_AIN5P_AIN5M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(5, 5, 5, AD7795_CH_AIN6P_AIN6M, (_b), (_sb), 0), \
+	AD7793_SHORTED_CHANNEL(6, 0, AD7795_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
+	AD7793_TEMP_CHANNEL(7, AD7793_CH_TEMP, (_b), (_sb), 0), \
+	AD7793_SUPPLY_CHANNEL(8, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
 	IIO_CHAN_SOFT_TIMESTAMP(9), \
 }
 
 #define DECLARE_AD7797_CHANNELS(_name, _b, _sb) \
 const struct iio_chan_spec _name##_channels[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
-	AD_SD_SHORTED_CHANNEL(1, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
-	AD_SD_TEMP_CHANNEL(2, AD7793_CH_TEMP, (_b), (_sb), 0), \
-	AD_SD_SUPPLY_CHANNEL(3, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
+	AD7797_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
+	AD7797_SHORTED_CHANNEL(1, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
+	AD7793_TEMP_CHANNEL(2, AD7793_CH_TEMP, (_b), (_sb), 0), \
+	AD7793_SUPPLY_CHANNEL(3, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
 	IIO_CHAN_SOFT_TIMESTAMP(4), \
 }
 
 #define DECLARE_AD7799_CHANNELS(_name, _b, _sb) \
 const struct iio_chan_spec _name##_channels[] = { \
-	AD_SD_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), 0), \
-	AD_SD_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), 0), \
-	AD_SD_SHORTED_CHANNEL(3, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
-	AD_SD_SUPPLY_CHANNEL(4, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(0, 0, 0, AD7793_CH_AIN1P_AIN1M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(1, 1, 1, AD7793_CH_AIN2P_AIN2M, (_b), (_sb), 0), \
+	AD7793_DIFF_CHANNEL(2, 2, 2, AD7793_CH_AIN3P_AIN3M, (_b), (_sb), 0), \
+	AD7793_SHORTED_CHANNEL(3, 0, AD7793_CH_AIN1M_AIN1M, (_b), (_sb), 0), \
+	AD7793_SUPPLY_CHANNEL(4, 3, AD7793_CH_AVDD_MONITOR, (_b), (_sb), 0), \
 	IIO_CHAN_SOFT_TIMESTAMP(5), \
 }
 
diff --git a/drivers/iio/adc/ad9467.c b/drivers/iio/adc/ad9467.c
new file mode 100644
index 000000000000..1e8fd83b9bc2
--- /dev/null
+++ b/drivers/iio/adc/ad9467.c
@@ -0,0 +1,422 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Analog Devices AD9467 SPI ADC driver
+ *
+ * Copyright 2012-2020 Analog Devices Inc.
+ */
+
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/spi/spi.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/of_device.h>
+
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#include <linux/clk.h>
+
+#include <linux/iio/adc/adi-axi-adc.h>
+
+/*
+ * ADI High-Speed ADC common spi interface registers
+ * See Application-Note AN-877:
+ *   https://www.analog.com/media/en/technical-documentation/application-notes/AN-877.pdf
+ */
+
+#define AN877_ADC_REG_CHIP_PORT_CONF		0x00
+#define AN877_ADC_REG_CHIP_ID			0x01
+#define AN877_ADC_REG_CHIP_GRADE		0x02
+#define AN877_ADC_REG_CHAN_INDEX		0x05
+#define AN877_ADC_REG_TRANSFER			0xFF
+#define AN877_ADC_REG_MODES			0x08
+#define AN877_ADC_REG_TEST_IO			0x0D
+#define AN877_ADC_REG_ADC_INPUT			0x0F
+#define AN877_ADC_REG_OFFSET			0x10
+#define AN877_ADC_REG_OUTPUT_MODE		0x14
+#define AN877_ADC_REG_OUTPUT_ADJUST		0x15
+#define AN877_ADC_REG_OUTPUT_PHASE		0x16
+#define AN877_ADC_REG_OUTPUT_DELAY		0x17
+#define AN877_ADC_REG_VREF			0x18
+#define AN877_ADC_REG_ANALOG_INPUT		0x2C
+
+/* AN877_ADC_REG_TEST_IO */
+#define AN877_ADC_TESTMODE_OFF			0x0
+#define AN877_ADC_TESTMODE_MIDSCALE_SHORT	0x1
+#define AN877_ADC_TESTMODE_POS_FULLSCALE	0x2
+#define AN877_ADC_TESTMODE_NEG_FULLSCALE	0x3
+#define AN877_ADC_TESTMODE_ALT_CHECKERBOARD	0x4
+#define AN877_ADC_TESTMODE_PN23_SEQ		0x5
+#define AN877_ADC_TESTMODE_PN9_SEQ		0x6
+#define AN877_ADC_TESTMODE_ONE_ZERO_TOGGLE	0x7
+#define AN877_ADC_TESTMODE_USER			0x8
+#define AN877_ADC_TESTMODE_BIT_TOGGLE		0x9
+#define AN877_ADC_TESTMODE_SYNC			0xA
+#define AN877_ADC_TESTMODE_ONE_BIT_HIGH		0xB
+#define AN877_ADC_TESTMODE_MIXED_BIT_FREQUENCY	0xC
+#define AN877_ADC_TESTMODE_RAMP			0xF
+
+/* AN877_ADC_REG_TRANSFER */
+#define AN877_ADC_TRANSFER_SYNC			0x1
+
+/* AN877_ADC_REG_OUTPUT_MODE */
+#define AN877_ADC_OUTPUT_MODE_OFFSET_BINARY	0x0
+#define AN877_ADC_OUTPUT_MODE_TWOS_COMPLEMENT	0x1
+#define AN877_ADC_OUTPUT_MODE_GRAY_CODE		0x2
+
+/* AN877_ADC_REG_OUTPUT_PHASE */
+#define AN877_ADC_OUTPUT_EVEN_ODD_MODE_EN	0x20
+#define AN877_ADC_INVERT_DCO_CLK		0x80
+
+/* AN877_ADC_REG_OUTPUT_DELAY */
+#define AN877_ADC_DCO_DELAY_ENABLE		0x80
+
+/*
+ * Analog Devices AD9467 16-Bit, 200/250 MSPS ADC
+ */
+
+#define CHIPID_AD9467			0x50
+#define AD9467_DEF_OUTPUT_MODE		0x08
+#define AD9467_REG_VREF_MASK		0x0F
+
+enum {
+	ID_AD9467,
+};
+
+struct ad9467_state {
+	struct spi_device		*spi;
+	struct clk			*clk;
+	unsigned int			output_mode;
+
+	struct gpio_desc		*pwrdown_gpio;
+	struct gpio_desc		*reset_gpio;
+};
+
+static int ad9467_spi_read(struct spi_device *spi, unsigned int reg)
+{
+	unsigned char tbuf[2], rbuf[1];
+	int ret;
+
+	tbuf[0] = 0x80 | (reg >> 8);
+	tbuf[1] = reg & 0xFF;
+
+	ret = spi_write_then_read(spi,
+				  tbuf, ARRAY_SIZE(tbuf),
+				  rbuf, ARRAY_SIZE(rbuf));
+
+	if (ret < 0)
+		return ret;
+
+	return rbuf[0];
+}
+
+static int ad9467_spi_write(struct spi_device *spi, unsigned int reg,
+			    unsigned int val)
+{
+	unsigned char buf[3];
+
+	buf[0] = reg >> 8;
+	buf[1] = reg & 0xFF;
+	buf[2] = val;
+
+	return spi_write(spi, buf, ARRAY_SIZE(buf));
+}
+
+static int ad9467_reg_access(struct adi_axi_adc_conv *conv, unsigned int reg,
+			     unsigned int writeval, unsigned int *readval)
+{
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+	struct spi_device *spi = st->spi;
+	int ret;
+
+	if (readval == NULL) {
+		ret = ad9467_spi_write(spi, reg, writeval);
+		ad9467_spi_write(spi, AN877_ADC_REG_TRANSFER,
+				 AN877_ADC_TRANSFER_SYNC);
+		return ret;
+	}
+
+	ret = ad9467_spi_read(spi, reg);
+	if (ret < 0)
+		return ret;
+	*readval = ret;
+
+	return 0;
+}
+
+static const unsigned int ad9467_scale_table[][2] = {
+	{2000, 0}, {2100, 6}, {2200, 7},
+	{2300, 8}, {2400, 9}, {2500, 10},
+};
+
+static void __ad9467_get_scale(struct adi_axi_adc_conv *conv, int index,
+			       unsigned int *val, unsigned int *val2)
+{
+	const struct adi_axi_adc_chip_info *info = conv->chip_info;
+	const struct iio_chan_spec *chan = &info->channels[0];
+	unsigned int tmp;
+
+	tmp = (info->scale_table[index][0] * 1000000ULL) >>
+			chan->scan_type.realbits;
+	*val = tmp / 1000000;
+	*val2 = tmp % 1000000;
+}
+
+#define AD9467_CHAN(_chan, _si, _bits, _sign)				\
+{									\
+	.type = IIO_VOLTAGE,						\
+	.indexed = 1,							\
+	.channel = _chan,						\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\
+		BIT(IIO_CHAN_INFO_SAMP_FREQ),				\
+	.scan_index = _si,						\
+	.scan_type = {							\
+		.sign = _sign,						\
+		.realbits = _bits,					\
+		.storagebits = 16,					\
+	},								\
+}
+
+static const struct iio_chan_spec ad9467_channels[] = {
+	AD9467_CHAN(0, 0, 16, 'S'),
+};
+
+static const struct adi_axi_adc_chip_info ad9467_chip_tbl[] = {
+	[ID_AD9467] = {
+		.id = CHIPID_AD9467,
+		.max_rate = 250000000UL,
+		.scale_table = ad9467_scale_table,
+		.num_scales = ARRAY_SIZE(ad9467_scale_table),
+		.channels = ad9467_channels,
+		.num_channels = ARRAY_SIZE(ad9467_channels),
+	},
+};
+
+static int ad9467_get_scale(struct adi_axi_adc_conv *conv, int *val, int *val2)
+{
+	const struct adi_axi_adc_chip_info *info = conv->chip_info;
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+	unsigned int i, vref_val, vref_mask;
+
+	vref_val = ad9467_spi_read(st->spi, AN877_ADC_REG_VREF);
+
+	switch (info->id) {
+	case CHIPID_AD9467:
+		vref_mask = AD9467_REG_VREF_MASK;
+		break;
+	default:
+		vref_mask = 0xFFFF;
+		break;
+	}
+
+	vref_val &= vref_mask;
+
+	for (i = 0; i < info->num_scales; i++) {
+		if (vref_val == info->scale_table[i][1])
+			break;
+	}
+
+	if (i == info->num_scales)
+		return -ERANGE;
+
+	__ad9467_get_scale(conv, i, val, val2);
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int ad9467_set_scale(struct adi_axi_adc_conv *conv, int val, int val2)
+{
+	const struct adi_axi_adc_chip_info *info = conv->chip_info;
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+	unsigned int scale_val[2];
+	unsigned int i;
+
+	if (val != 0)
+		return -EINVAL;
+
+	for (i = 0; i < info->num_scales; i++) {
+		__ad9467_get_scale(conv, i, &scale_val[0], &scale_val[1]);
+		if (scale_val[0] != val || scale_val[1] != val2)
+			continue;
+
+		ad9467_spi_write(st->spi, AN877_ADC_REG_VREF,
+				 info->scale_table[i][1]);
+		ad9467_spi_write(st->spi, AN877_ADC_REG_TRANSFER,
+				 AN877_ADC_TRANSFER_SYNC);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+static int ad9467_read_raw(struct adi_axi_adc_conv *conv,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long m)
+{
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+
+	switch (m) {
+	case IIO_CHAN_INFO_SCALE:
+		return ad9467_get_scale(conv, val, val2);
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val = clk_get_rate(st->clk);
+
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ad9467_write_raw(struct adi_axi_adc_conv *conv,
+			    struct iio_chan_spec const *chan,
+			    int val, int val2, long mask)
+{
+	const struct adi_axi_adc_chip_info *info = conv->chip_info;
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+	long r_clk;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		return ad9467_set_scale(conv, val, val2);
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		r_clk = clk_round_rate(st->clk, val);
+		if (r_clk < 0 || r_clk > info->max_rate) {
+			dev_warn(&st->spi->dev,
+				 "Error setting ADC sample rate %ld", r_clk);
+			return -EINVAL;
+		}
+
+		return clk_set_rate(st->clk, r_clk);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int ad9467_outputmode_set(struct spi_device *spi, unsigned int mode)
+{
+	int ret;
+
+	ret = ad9467_spi_write(spi, AN877_ADC_REG_OUTPUT_MODE, mode);
+	if (ret < 0)
+		return ret;
+
+	return ad9467_spi_write(spi, AN877_ADC_REG_TRANSFER,
+				AN877_ADC_TRANSFER_SYNC);
+}
+
+static int ad9467_preenable_setup(struct adi_axi_adc_conv *conv)
+{
+	struct ad9467_state *st = adi_axi_adc_conv_priv(conv);
+
+	return ad9467_outputmode_set(st->spi, st->output_mode);
+}
+
+static int ad9467_setup(struct ad9467_state *st, unsigned int chip_id)
+{
+	switch (chip_id) {
+	case CHIPID_AD9467:
+		st->output_mode = AD9467_DEF_OUTPUT_MODE |
+				  AN877_ADC_OUTPUT_MODE_TWOS_COMPLEMENT;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+static void ad9467_clk_disable(void *data)
+{
+	struct ad9467_state *st = data;
+
+	clk_disable_unprepare(st->clk);
+}
+
+static int ad9467_probe(struct spi_device *spi)
+{
+	const struct adi_axi_adc_chip_info *info;
+	struct adi_axi_adc_conv *conv;
+	struct ad9467_state *st;
+	unsigned int id;
+	int ret;
+
+	info = of_device_get_match_data(&spi->dev);
+	if (!info)
+		return -ENODEV;
+
+	conv = devm_adi_axi_adc_conv_register(&spi->dev, sizeof(*st));
+	if (IS_ERR(conv))
+		return PTR_ERR(conv);
+
+	st = adi_axi_adc_conv_priv(conv);
+	st->spi = spi;
+
+	st->clk = devm_clk_get(&spi->dev, "adc-clk");
+	if (IS_ERR(st->clk))
+		return PTR_ERR(st->clk);
+
+	ret = clk_prepare_enable(st->clk);
+	if (ret < 0)
+		return ret;
+
+	ret = devm_add_action_or_reset(&spi->dev, ad9467_clk_disable, st);
+	if (ret)
+		return ret;
+
+	st->pwrdown_gpio = devm_gpiod_get_optional(&spi->dev, "powerdown",
+						   GPIOD_OUT_LOW);
+	if (IS_ERR(st->pwrdown_gpio))
+		return PTR_ERR(st->pwrdown_gpio);
+
+	st->reset_gpio = devm_gpiod_get_optional(&spi->dev, "reset",
+						 GPIOD_OUT_LOW);
+	if (IS_ERR(st->reset_gpio))
+		return PTR_ERR(st->reset_gpio);
+
+	if (st->reset_gpio) {
+		udelay(1);
+		ret = gpiod_direction_output(st->reset_gpio, 1);
+		if (ret)
+			return ret;
+		mdelay(10);
+	}
+
+	spi_set_drvdata(spi, st);
+
+	conv->chip_info = info;
+
+	id = ad9467_spi_read(spi, AN877_ADC_REG_CHIP_ID);
+	if (id != conv->chip_info->id) {
+		dev_err(&spi->dev, "Unrecognized CHIP_ID 0x%X\n", id);
+		return -ENODEV;
+	}
+
+	conv->reg_access = ad9467_reg_access;
+	conv->write_raw = ad9467_write_raw;
+	conv->read_raw = ad9467_read_raw;
+	conv->preenable_setup = ad9467_preenable_setup;
+
+	return ad9467_setup(st, id);
+}
+
+static const struct of_device_id ad9467_of_match[] = {
+	{ .compatible = "adi,ad9467", .data = &ad9467_chip_tbl[ID_AD9467], },
+	{}
+};
+MODULE_DEVICE_TABLE(of, ad9467_of_match);
+
+static struct spi_driver ad9467_driver = {
+	.driver = {
+		.name = "ad9467",
+		.of_match_table = ad9467_of_match,
+	},
+	.probe = ad9467_probe,
+};
+module_spi_driver(ad9467_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Analog Devices AD9467 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ad_sigma_delta.c b/drivers/iio/adc/ad_sigma_delta.c
index 8115b6de1d6c..dd3d54b3bc8b 100644
--- a/drivers/iio/adc/ad_sigma_delta.c
+++ b/drivers/iio/adc/ad_sigma_delta.c
@@ -70,9 +70,7 @@ int ad_sd_write_reg(struct ad_sigma_delta *sigma_delta, unsigned int reg,
 
 	switch (size) {
 	case 3:
-		data[1] = val >> 16;
-		data[2] = val >> 8;
-		data[3] = val;
+		put_unaligned_be24(val, &data[1]);
 		break;
 	case 2:
 		put_unaligned_be16(val, &data[1]);
@@ -157,9 +155,7 @@ int ad_sd_read_reg(struct ad_sigma_delta *sigma_delta,
 		*val = get_unaligned_be32(sigma_delta->data);
 		break;
 	case 3:
-		*val = (sigma_delta->data[0] << 16) |
-			(sigma_delta->data[1] << 8) |
-			sigma_delta->data[2];
+		*val = get_unaligned_be24(&sigma_delta->data[0]);
 		break;
 	case 2:
 		*val = get_unaligned_be16(sigma_delta->data);
diff --git a/drivers/iio/adc/adi-axi-adc.c b/drivers/iio/adc/adi-axi-adc.c
new file mode 100644
index 000000000000..c24c8da99eb4
--- /dev/null
+++ b/drivers/iio/adc/adi-axi-adc.c
@@ -0,0 +1,482 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Analog Devices Generic AXI ADC IP core
+ * Link: https://wiki.analog.com/resources/fpga/docs/axi_adc_ip
+ *
+ * Copyright 2012-2020 Analog Devices Inc.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/buffer-dmaengine.h>
+
+#include <linux/fpga/adi-axi-common.h>
+#include <linux/iio/adc/adi-axi-adc.h>
+
+/**
+ * Register definitions:
+ *   https://wiki.analog.com/resources/fpga/docs/axi_adc_ip#register_map
+ */
+
+/* ADC controls */
+
+#define ADI_AXI_REG_RSTN			0x0040
+#define   ADI_AXI_REG_RSTN_CE_N			BIT(2)
+#define   ADI_AXI_REG_RSTN_MMCM_RSTN		BIT(1)
+#define   ADI_AXI_REG_RSTN_RSTN			BIT(0)
+
+/* ADC Channel controls */
+
+#define ADI_AXI_REG_CHAN_CTRL(c)		(0x0400 + (c) * 0x40)
+#define   ADI_AXI_REG_CHAN_CTRL_LB_OWR		BIT(11)
+#define   ADI_AXI_REG_CHAN_CTRL_PN_SEL_OWR	BIT(10)
+#define   ADI_AXI_REG_CHAN_CTRL_IQCOR_EN	BIT(9)
+#define   ADI_AXI_REG_CHAN_CTRL_DCFILT_EN	BIT(8)
+#define   ADI_AXI_REG_CHAN_CTRL_FMT_SIGNEXT	BIT(6)
+#define   ADI_AXI_REG_CHAN_CTRL_FMT_TYPE	BIT(5)
+#define   ADI_AXI_REG_CHAN_CTRL_FMT_EN		BIT(4)
+#define   ADI_AXI_REG_CHAN_CTRL_PN_TYPE_OWR	BIT(1)
+#define   ADI_AXI_REG_CHAN_CTRL_ENABLE		BIT(0)
+
+#define ADI_AXI_REG_CHAN_CTRL_DEFAULTS		\
+	(ADI_AXI_REG_CHAN_CTRL_FMT_SIGNEXT |	\
+	 ADI_AXI_REG_CHAN_CTRL_FMT_EN |		\
+	 ADI_AXI_REG_CHAN_CTRL_ENABLE)
+
+struct adi_axi_adc_core_info {
+	unsigned int				version;
+};
+
+struct adi_axi_adc_state {
+	struct mutex				lock;
+
+	struct adi_axi_adc_client		*client;
+	void __iomem				*regs;
+};
+
+struct adi_axi_adc_client {
+	struct list_head			entry;
+	struct adi_axi_adc_conv			conv;
+	struct adi_axi_adc_state		*state;
+	struct device				*dev;
+	const struct adi_axi_adc_core_info	*info;
+};
+
+static LIST_HEAD(registered_clients);
+static DEFINE_MUTEX(registered_clients_lock);
+
+static struct adi_axi_adc_client *conv_to_client(struct adi_axi_adc_conv *conv)
+{
+	return container_of(conv, struct adi_axi_adc_client, conv);
+}
+
+void *adi_axi_adc_conv_priv(struct adi_axi_adc_conv *conv)
+{
+	struct adi_axi_adc_client *cl = conv_to_client(conv);
+
+	return (char *)cl + ALIGN(sizeof(struct adi_axi_adc_client), IIO_ALIGN);
+}
+EXPORT_SYMBOL_GPL(adi_axi_adc_conv_priv);
+
+static void adi_axi_adc_write(struct adi_axi_adc_state *st,
+			      unsigned int reg,
+			      unsigned int val)
+{
+	iowrite32(val, st->regs + reg);
+}
+
+static unsigned int adi_axi_adc_read(struct adi_axi_adc_state *st,
+				     unsigned int reg)
+{
+	return ioread32(st->regs + reg);
+}
+
+static int adi_axi_adc_config_dma_buffer(struct device *dev,
+					 struct iio_dev *indio_dev)
+{
+	struct iio_buffer *buffer;
+	const char *dma_name;
+
+	if (!device_property_present(dev, "dmas"))
+		return 0;
+
+	if (device_property_read_string(dev, "dma-names", &dma_name))
+		dma_name = "rx";
+
+	buffer = devm_iio_dmaengine_buffer_alloc(indio_dev->dev.parent,
+						 dma_name);
+	if (IS_ERR(buffer))
+		return PTR_ERR(buffer);
+
+	indio_dev->modes |= INDIO_BUFFER_HARDWARE;
+	iio_device_attach_buffer(indio_dev, buffer);
+
+	return 0;
+}
+
+static int adi_axi_adc_read_raw(struct iio_dev *indio_dev,
+				struct iio_chan_spec const *chan,
+				int *val, int *val2, long mask)
+{
+	struct adi_axi_adc_state *st = iio_priv(indio_dev);
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+
+	if (!conv->read_raw)
+		return -EOPNOTSUPP;
+
+	return conv->read_raw(conv, chan, val, val2, mask);
+}
+
+static int adi_axi_adc_write_raw(struct iio_dev *indio_dev,
+				 struct iio_chan_spec const *chan,
+				 int val, int val2, long mask)
+{
+	struct adi_axi_adc_state *st = iio_priv(indio_dev);
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+
+	if (!conv->write_raw)
+		return -EOPNOTSUPP;
+
+	return conv->write_raw(conv, chan, val, val2, mask);
+}
+
+static int adi_axi_adc_update_scan_mode(struct iio_dev *indio_dev,
+					const unsigned long *scan_mask)
+{
+	struct adi_axi_adc_state *st = iio_priv(indio_dev);
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+	unsigned int i, ctrl;
+
+	for (i = 0; i < conv->chip_info->num_channels; i++) {
+		ctrl = adi_axi_adc_read(st, ADI_AXI_REG_CHAN_CTRL(i));
+
+		if (test_bit(i, scan_mask))
+			ctrl |= ADI_AXI_REG_CHAN_CTRL_ENABLE;
+		else
+			ctrl &= ~ADI_AXI_REG_CHAN_CTRL_ENABLE;
+
+		adi_axi_adc_write(st, ADI_AXI_REG_CHAN_CTRL(i), ctrl);
+	}
+
+	return 0;
+}
+
+static struct adi_axi_adc_conv *adi_axi_adc_conv_register(struct device *dev,
+							  size_t sizeof_priv)
+{
+	struct adi_axi_adc_client *cl;
+	size_t alloc_size;
+
+	alloc_size = ALIGN(sizeof(struct adi_axi_adc_client), IIO_ALIGN);
+	if (sizeof_priv)
+		alloc_size += ALIGN(sizeof_priv, IIO_ALIGN);
+
+	cl = kzalloc(alloc_size, GFP_KERNEL);
+	if (!cl)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_lock(&registered_clients_lock);
+
+	cl->dev = get_device(dev);
+
+	list_add_tail(&cl->entry, &registered_clients);
+
+	mutex_unlock(&registered_clients_lock);
+
+	return &cl->conv;
+}
+
+static void adi_axi_adc_conv_unregister(struct adi_axi_adc_conv *conv)
+{
+	struct adi_axi_adc_client *cl = conv_to_client(conv);
+
+	mutex_lock(&registered_clients_lock);
+
+	list_del(&cl->entry);
+	put_device(cl->dev);
+
+	mutex_unlock(&registered_clients_lock);
+
+	kfree(cl);
+}
+
+static void devm_adi_axi_adc_conv_release(struct device *dev, void *res)
+{
+	adi_axi_adc_conv_unregister(*(struct adi_axi_adc_conv **)res);
+}
+
+struct adi_axi_adc_conv *devm_adi_axi_adc_conv_register(struct device *dev,
+							size_t sizeof_priv)
+{
+	struct adi_axi_adc_conv **ptr, *conv;
+
+	ptr = devres_alloc(devm_adi_axi_adc_conv_release, sizeof(*ptr),
+			   GFP_KERNEL);
+	if (!ptr)
+		return ERR_PTR(-ENOMEM);
+
+	conv = adi_axi_adc_conv_register(dev, sizeof_priv);
+	if (IS_ERR(conv)) {
+		devres_free(ptr);
+		return ERR_CAST(conv);
+	}
+
+	*ptr = conv;
+	devres_add(dev, ptr);
+
+	return conv;
+}
+EXPORT_SYMBOL_GPL(devm_adi_axi_adc_conv_register);
+
+static ssize_t in_voltage_scale_available_show(struct device *dev,
+					       struct device_attribute *attr,
+					       char *buf)
+{
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct adi_axi_adc_state *st = iio_priv(indio_dev);
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+	size_t len = 0;
+	int i;
+
+	for (i = 0; i < conv->chip_info->num_scales; i++) {
+		const unsigned int *s = conv->chip_info->scale_table[i];
+
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+				 "%u.%06u ", s[0], s[1]);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static IIO_DEVICE_ATTR_RO(in_voltage_scale_available, 0);
+
+enum {
+	ADI_AXI_ATTR_SCALE_AVAIL,
+};
+
+#define ADI_AXI_ATTR(_en_, _file_)			\
+	[ADI_AXI_ATTR_##_en_] = &iio_dev_attr_##_file_.dev_attr.attr
+
+static struct attribute *adi_axi_adc_attributes[] = {
+	ADI_AXI_ATTR(SCALE_AVAIL, in_voltage_scale_available),
+	NULL
+};
+
+static umode_t axi_adc_attr_is_visible(struct kobject *kobj,
+				       struct attribute *attr, int n)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct adi_axi_adc_state *st = iio_priv(indio_dev);
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+
+	switch (n) {
+	case ADI_AXI_ATTR_SCALE_AVAIL:
+		if (!conv->chip_info->num_scales)
+			return 0;
+		return attr->mode;
+	default:
+		return attr->mode;
+	}
+}
+
+static const struct attribute_group adi_axi_adc_attribute_group = {
+	.attrs = adi_axi_adc_attributes,
+	.is_visible = axi_adc_attr_is_visible,
+};
+
+static const struct iio_info adi_axi_adc_info = {
+	.read_raw = &adi_axi_adc_read_raw,
+	.write_raw = &adi_axi_adc_write_raw,
+	.attrs = &adi_axi_adc_attribute_group,
+	.update_scan_mode = &adi_axi_adc_update_scan_mode,
+};
+
+static const struct adi_axi_adc_core_info adi_axi_adc_10_0_a_info = {
+	.version = ADI_AXI_PCORE_VER(10, 0, 'a'),
+};
+
+static struct adi_axi_adc_client *adi_axi_adc_attach_client(struct device *dev)
+{
+	const struct adi_axi_adc_core_info *info;
+	struct adi_axi_adc_client *cl;
+	struct device_node *cln;
+
+	info = of_device_get_match_data(dev);
+	if (!info)
+		return ERR_PTR(-ENODEV);
+
+	cln = of_parse_phandle(dev->of_node, "adi,adc-dev", 0);
+	if (!cln) {
+		dev_err(dev, "No 'adi,adc-dev' node defined\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	mutex_lock(&registered_clients_lock);
+
+	list_for_each_entry(cl, &registered_clients, entry) {
+		if (!cl->dev)
+			continue;
+
+		if (cl->dev->of_node != cln)
+			continue;
+
+		if (!try_module_get(dev->driver->owner)) {
+			mutex_unlock(&registered_clients_lock);
+			return ERR_PTR(-ENODEV);
+		}
+
+		get_device(dev);
+		cl->info = info;
+		mutex_unlock(&registered_clients_lock);
+		return cl;
+	}
+
+	mutex_unlock(&registered_clients_lock);
+
+	return ERR_PTR(-EPROBE_DEFER);
+}
+
+static int adi_axi_adc_setup_channels(struct device *dev,
+				      struct adi_axi_adc_state *st)
+{
+	struct adi_axi_adc_conv *conv = &st->client->conv;
+	int i, ret;
+
+	if (conv->preenable_setup) {
+		ret = conv->preenable_setup(conv);
+		if (ret)
+			return ret;
+	}
+
+	for (i = 0; i < conv->chip_info->num_channels; i++) {
+		adi_axi_adc_write(st, ADI_AXI_REG_CHAN_CTRL(i),
+				  ADI_AXI_REG_CHAN_CTRL_DEFAULTS);
+	}
+
+	return 0;
+}
+
+static void axi_adc_reset(struct adi_axi_adc_state *st)
+{
+	adi_axi_adc_write(st, ADI_AXI_REG_RSTN, 0);
+	mdelay(10);
+	adi_axi_adc_write(st, ADI_AXI_REG_RSTN, ADI_AXI_REG_RSTN_MMCM_RSTN);
+	mdelay(10);
+	adi_axi_adc_write(st, ADI_AXI_REG_RSTN,
+			  ADI_AXI_REG_RSTN_RSTN | ADI_AXI_REG_RSTN_MMCM_RSTN);
+}
+
+static void adi_axi_adc_cleanup(void *data)
+{
+	struct adi_axi_adc_client *cl = data;
+
+	put_device(cl->dev);
+	module_put(cl->dev->driver->owner);
+}
+
+static int adi_axi_adc_probe(struct platform_device *pdev)
+{
+	struct adi_axi_adc_conv *conv;
+	struct iio_dev *indio_dev;
+	struct adi_axi_adc_client *cl;
+	struct adi_axi_adc_state *st;
+	unsigned int ver;
+	int ret;
+
+	cl = adi_axi_adc_attach_client(&pdev->dev);
+	if (IS_ERR(cl))
+		return PTR_ERR(cl);
+
+	ret = devm_add_action_or_reset(&pdev->dev, adi_axi_adc_cleanup, cl);
+	if (ret)
+		return ret;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*st));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	st = iio_priv(indio_dev);
+	st->client = cl;
+	cl->state = st;
+	mutex_init(&st->lock);
+
+	st->regs = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(st->regs))
+		return PTR_ERR(st->regs);
+
+	conv = &st->client->conv;
+
+	axi_adc_reset(st);
+
+	ver = adi_axi_adc_read(st, ADI_AXI_REG_VERSION);
+
+	if (cl->info->version > ver) {
+		dev_err(&pdev->dev,
+			"IP core version is too old. Expected %d.%.2d.%c, Reported %d.%.2d.%c\n",
+			ADI_AXI_PCORE_VER_MAJOR(cl->info->version),
+			ADI_AXI_PCORE_VER_MINOR(cl->info->version),
+			ADI_AXI_PCORE_VER_PATCH(cl->info->version),
+			ADI_AXI_PCORE_VER_MAJOR(ver),
+			ADI_AXI_PCORE_VER_MINOR(ver),
+			ADI_AXI_PCORE_VER_PATCH(ver));
+		return -ENODEV;
+	}
+
+	indio_dev->info = &adi_axi_adc_info;
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->name = "adi-axi-adc";
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->num_channels = conv->chip_info->num_channels;
+	indio_dev->channels = conv->chip_info->channels;
+
+	ret = adi_axi_adc_config_dma_buffer(&pdev->dev, indio_dev);
+	if (ret)
+		return ret;
+
+	ret = adi_axi_adc_setup_channels(&pdev->dev, st);
+	if (ret)
+		return ret;
+
+	ret = devm_iio_device_register(&pdev->dev, indio_dev);
+	if (ret)
+		return ret;
+
+	dev_info(&pdev->dev, "AXI ADC IP core (%d.%.2d.%c) probed\n",
+		 ADI_AXI_PCORE_VER_MAJOR(ver),
+		 ADI_AXI_PCORE_VER_MINOR(ver),
+		 ADI_AXI_PCORE_VER_PATCH(ver));
+
+	return 0;
+}
+
+/* Match table for of_platform binding */
+static const struct of_device_id adi_axi_adc_of_match[] = {
+	{ .compatible = "adi,axi-adc-10.0.a", .data = &adi_axi_adc_10_0_a_info },
+	{ /* end of list */ }
+};
+MODULE_DEVICE_TABLE(of, adi_axi_adc_of_match);
+
+static struct platform_driver adi_axi_adc_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.of_match_table = adi_axi_adc_of_match,
+	},
+	.probe = adi_axi_adc_probe,
+};
+module_platform_driver(adi_axi_adc_driver);
+
+MODULE_AUTHOR("Michael Hennerich <michael.hennerich@analog.com>");
+MODULE_DESCRIPTION("Analog Devices Generic AXI ADC IP core driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/at91-sama5d2_adc.c b/drivers/iio/adc/at91-sama5d2_adc.c
index 9d96f7d08b95..9abbbdcc7420 100644
--- a/drivers/iio/adc/at91-sama5d2_adc.c
+++ b/drivers/iio/adc/at91-sama5d2_adc.c
@@ -8,6 +8,7 @@
 
 #include <linux/bitops.h>
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/dmaengine.h>
 #include <linux/interrupt.h>
@@ -100,6 +101,8 @@
 #define AT91_SAMA5D2_IER_YRDY   BIT(21)
 /* Interrupt Enable Register - TS pressure measurement ready */
 #define AT91_SAMA5D2_IER_PRDY   BIT(22)
+/* Interrupt Enable Register - Data ready */
+#define AT91_SAMA5D2_IER_DRDY   BIT(24)
 /* Interrupt Enable Register - general overrun error */
 #define AT91_SAMA5D2_IER_GOVRE BIT(25)
 /* Interrupt Enable Register - Pen detect */
@@ -486,6 +489,21 @@ static inline int at91_adc_of_xlate(struct iio_dev *indio_dev,
 	return at91_adc_chan_xlate(indio_dev, iiospec->args[0]);
 }
 
+static unsigned int at91_adc_active_scan_mask_to_reg(struct iio_dev *indio_dev)
+{
+	u32 mask = 0;
+	u8 bit;
+
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->num_channels) {
+		struct iio_chan_spec const *chan =
+			 at91_adc_chan_get(indio_dev, bit);
+		mask |= BIT(chan->channel);
+	}
+
+	return mask & GENMASK(11, 0);
+}
+
 static void at91_adc_config_emr(struct at91_adc_state *st)
 {
 	/* configure the extended mode register */
@@ -710,7 +728,6 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 	struct iio_dev *indio = iio_trigger_get_drvdata(trig);
 	struct at91_adc_state *st = iio_priv(indio);
 	u32 status = at91_adc_readl(st, AT91_SAMA5D2_TRGR);
-	u8 bit;
 
 	/* clear TRGMOD */
 	status &= ~AT91_SAMA5D2_TRGR_TRGMOD_MASK;
@@ -721,50 +738,6 @@ static int at91_adc_configure_trigger(struct iio_trigger *trig, bool state)
 	/* set/unset hw trigger */
 	at91_adc_writel(st, AT91_SAMA5D2_TRGR, status);
 
-	for_each_set_bit(bit, indio->active_scan_mask, indio->num_channels) {
-		struct iio_chan_spec const *chan = at91_adc_chan_get(indio, bit);
-		u32 cor;
-
-		if (!chan)
-			continue;
-		/* these channel types cannot be handled by this trigger */
-		if (chan->type == IIO_POSITIONRELATIVE ||
-		    chan->type == IIO_PRESSURE)
-			continue;
-
-		if (state) {
-			cor = at91_adc_readl(st, AT91_SAMA5D2_COR);
-
-			if (chan->differential)
-				cor |= (BIT(chan->channel) |
-					BIT(chan->channel2)) <<
-					AT91_SAMA5D2_COR_DIFF_OFFSET;
-			else
-				cor &= ~(BIT(chan->channel) <<
-				       AT91_SAMA5D2_COR_DIFF_OFFSET);
-
-			at91_adc_writel(st, AT91_SAMA5D2_COR, cor);
-		}
-
-		if (state) {
-			at91_adc_writel(st, AT91_SAMA5D2_CHER,
-					BIT(chan->channel));
-			/* enable irq only if not using DMA */
-			if (!st->dma_st.dma_chan) {
-				at91_adc_writel(st, AT91_SAMA5D2_IER,
-						BIT(chan->channel));
-			}
-		} else {
-			/* disable irq only if not using DMA */
-			if (!st->dma_st.dma_chan) {
-				at91_adc_writel(st, AT91_SAMA5D2_IDR,
-						BIT(chan->channel));
-			}
-			at91_adc_writel(st, AT91_SAMA5D2_CHDR,
-					BIT(chan->channel));
-		}
-	}
-
 	return 0;
 }
 
@@ -781,6 +754,7 @@ static int at91_adc_reenable_trigger(struct iio_trigger *trig)
 
 	/* Needed to ACK the DRDY interruption */
 	at91_adc_readl(st, AT91_SAMA5D2_LCDR);
+
 	return 0;
 }
 
@@ -888,18 +862,37 @@ static int at91_adc_dma_start(struct iio_dev *indio_dev)
 	return 0;
 }
 
-static int at91_adc_buffer_postenable(struct iio_dev *indio_dev)
+static bool at91_adc_buffer_check_use_irq(struct iio_dev *indio,
+					  struct at91_adc_state *st)
+{
+	/* if using DMA, we do not use our own IRQ (we use DMA-controller) */
+	if (st->dma_st.dma_chan)
+		return false;
+	/* if the trigger is not ours, then it has its own IRQ */
+	if (iio_trigger_validate_own_device(indio->trig, indio))
+		return false;
+	return true;
+}
+
+static bool at91_adc_current_chan_is_touch(struct iio_dev *indio_dev)
+{
+	struct at91_adc_state *st = iio_priv(indio_dev);
+
+	return !!bitmap_subset(indio_dev->active_scan_mask,
+			       &st->touch_st.channels_bitmask,
+			       AT91_SAMA5D2_MAX_CHAN_IDX + 1);
+}
+
+static int at91_adc_buffer_preenable(struct iio_dev *indio_dev)
 {
 	int ret;
+	u8 bit;
 	struct at91_adc_state *st = iio_priv(indio_dev);
 
 	/* check if we are enabling triggered buffer or the touchscreen */
-	if (bitmap_subset(indio_dev->active_scan_mask,
-			  &st->touch_st.channels_bitmask,
-			  AT91_SAMA5D2_MAX_CHAN_IDX + 1)) {
-		/* touchscreen enabling */
+	if (at91_adc_current_chan_is_touch(indio_dev))
 		return at91_adc_configure_touch(st, true);
-	}
+
 	/* if we are not in triggered mode, we cannot enable the buffer. */
 	if (!(indio_dev->currentmode & INDIO_ALL_TRIGGERED_MODES))
 		return -EINVAL;
@@ -911,41 +904,65 @@ static int at91_adc_buffer_postenable(struct iio_dev *indio_dev)
 		return ret;
 	}
 
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->num_channels) {
+		struct iio_chan_spec const *chan =
+					at91_adc_chan_get(indio_dev, bit);
+		u32 cor;
+
+		if (!chan)
+			continue;
+		/* these channel types cannot be handled by this trigger */
+		if (chan->type == IIO_POSITIONRELATIVE ||
+		    chan->type == IIO_PRESSURE)
+			continue;
+
+		cor = at91_adc_readl(st, AT91_SAMA5D2_COR);
+
+		if (chan->differential)
+			cor |= (BIT(chan->channel) | BIT(chan->channel2)) <<
+				AT91_SAMA5D2_COR_DIFF_OFFSET;
+		else
+			cor &= ~(BIT(chan->channel) <<
+			       AT91_SAMA5D2_COR_DIFF_OFFSET);
+
+		at91_adc_writel(st, AT91_SAMA5D2_COR, cor);
+
+		at91_adc_writel(st, AT91_SAMA5D2_CHER, BIT(chan->channel));
+	}
+
+	if (at91_adc_buffer_check_use_irq(indio_dev, st))
+		at91_adc_writel(st, AT91_SAMA5D2_IER, AT91_SAMA5D2_IER_DRDY);
+
+	return 0;
+}
+
+static int at91_adc_buffer_postenable(struct iio_dev *indio_dev)
+{
+	if (at91_adc_current_chan_is_touch(indio_dev))
+		return 0;
+
 	return iio_triggered_buffer_postenable(indio_dev);
 }
 
-static int at91_adc_buffer_predisable(struct iio_dev *indio_dev)
+static int at91_adc_buffer_postdisable(struct iio_dev *indio_dev)
 {
 	struct at91_adc_state *st = iio_priv(indio_dev);
-	int ret;
 	u8 bit;
 
 	/* check if we are disabling triggered buffer or the touchscreen */
-	if (bitmap_subset(indio_dev->active_scan_mask,
-			  &st->touch_st.channels_bitmask,
-			  AT91_SAMA5D2_MAX_CHAN_IDX + 1)) {
-		/* touchscreen disable */
+	if (at91_adc_current_chan_is_touch(indio_dev))
 		return at91_adc_configure_touch(st, false);
-	}
+
 	/* if we are not in triggered mode, nothing to do here */
 	if (!(indio_dev->currentmode & INDIO_ALL_TRIGGERED_MODES))
 		return -EINVAL;
 
-	/* continue with the triggered buffer */
-	ret = iio_triggered_buffer_predisable(indio_dev);
-	if (ret < 0)
-		dev_err(&indio_dev->dev, "buffer predisable failed\n");
-
-	if (!st->dma_st.dma_chan)
-		return ret;
-
-	/* if we are using DMA we must clear registers and end DMA */
-	dmaengine_terminate_sync(st->dma_st.dma_chan);
-
 	/*
-	 * For each enabled channel we must read the last converted value
+	 * For each enable channel we must disable it in hardware.
+	 * In the case of DMA, we must read the last converted value
 	 * to clear EOC status and not get a possible interrupt later.
-	 * This value is being read by DMA from LCDR anyway
+	 * This value is being read by DMA from LCDR anyway, so it's not lost.
 	 */
 	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->num_channels) {
@@ -958,16 +975,37 @@ static int at91_adc_buffer_predisable(struct iio_dev *indio_dev)
 		if (chan->type == IIO_POSITIONRELATIVE ||
 		    chan->type == IIO_PRESSURE)
 			continue;
+
+		at91_adc_writel(st, AT91_SAMA5D2_CHDR, BIT(chan->channel));
+
 		if (st->dma_st.dma_chan)
 			at91_adc_readl(st, chan->address);
 	}
 
+	if (at91_adc_buffer_check_use_irq(indio_dev, st))
+		at91_adc_writel(st, AT91_SAMA5D2_IDR, AT91_SAMA5D2_IER_DRDY);
+
 	/* read overflow register to clear possible overflow status */
 	at91_adc_readl(st, AT91_SAMA5D2_OVER);
-	return ret;
+
+	/* if we are using DMA we must clear registers and end DMA */
+	if (st->dma_st.dma_chan)
+		dmaengine_terminate_sync(st->dma_st.dma_chan);
+
+	return 0;
+}
+
+static int at91_adc_buffer_predisable(struct iio_dev *indio_dev)
+{
+	if (at91_adc_current_chan_is_touch(indio_dev))
+		return 0;
+
+	return iio_triggered_buffer_predisable(indio_dev);
 }
 
 static const struct iio_buffer_setup_ops at91_buffer_setup_ops = {
+	.preenable = &at91_adc_buffer_preenable,
+	.postdisable = &at91_adc_buffer_postdisable,
 	.postenable = &at91_adc_buffer_postenable,
 	.predisable = &at91_adc_buffer_predisable,
 };
@@ -1015,6 +1053,22 @@ static void at91_adc_trigger_handler_nodma(struct iio_dev *indio_dev,
 	int i = 0;
 	int val;
 	u8 bit;
+	u32 mask = at91_adc_active_scan_mask_to_reg(indio_dev);
+	unsigned int timeout = 50;
+
+	/*
+	 * Check if the conversion is ready. If not, wait a little bit, and
+	 * in case of timeout exit with an error.
+	 */
+	while ((at91_adc_readl(st, AT91_SAMA5D2_ISR) & mask) != mask &&
+	       timeout) {
+		usleep_range(50, 100);
+		timeout--;
+	}
+
+	/* Cannot read data, not ready. Continue without reporting data */
+	if (!timeout)
+		return;
 
 	for_each_set_bit(bit, indio_dev->active_scan_mask,
 			 indio_dev->num_channels) {
@@ -1102,6 +1156,13 @@ static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct at91_adc_state *st = iio_priv(indio_dev);
 
+	/*
+	 * If it's not our trigger, start a conversion now, as we are
+	 * actually polling the trigger now.
+	 */
+	if (iio_trigger_validate_own_device(indio_dev->trig, indio_dev))
+		at91_adc_writel(st, AT91_SAMA5D2_CR, AT91_SAMA5D2_CR_START);
+
 	if (st->dma_st.dma_chan)
 		at91_adc_trigger_handler_dma(indio_dev);
 	else
@@ -1114,20 +1175,9 @@ static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
 
 static int at91_adc_buffer_init(struct iio_dev *indio)
 {
-	struct at91_adc_state *st = iio_priv(indio);
-
-	if (st->selected_trig->hw_trig) {
-		return devm_iio_triggered_buffer_setup(&indio->dev, indio,
-			&iio_pollfunc_store_time,
-			&at91_adc_trigger_handler, &at91_buffer_setup_ops);
-	}
-	/*
-	 * we need to prepare the buffer ops in case we will get
-	 * another buffer attached (like a callback buffer for the touchscreen)
-	 */
-	indio->setup_ops = &at91_buffer_setup_ops;
-
-	return 0;
+	return devm_iio_triggered_buffer_setup(&indio->dev, indio,
+		&iio_pollfunc_store_time,
+		&at91_adc_trigger_handler, &at91_buffer_setup_ops);
 }
 
 static unsigned at91_adc_startup_time(unsigned startup_time_min,
@@ -1281,7 +1331,8 @@ static irqreturn_t at91_adc_interrupt(int irq, void *private)
 		status = at91_adc_readl(st, AT91_SAMA5D2_XPOSR);
 		status = at91_adc_readl(st, AT91_SAMA5D2_YPOSR);
 		status = at91_adc_readl(st, AT91_SAMA5D2_PRESSR);
-	} else if (iio_buffer_enabled(indio) && !st->dma_st.dma_chan) {
+	} else if (iio_buffer_enabled(indio) &&
+		   (status & AT91_SAMA5D2_IER_DRDY)) {
 		/* triggered buffer without DMA */
 		disable_irq_nosync(irq);
 		iio_trigger_poll(indio->trig);
@@ -1901,14 +1952,10 @@ static __maybe_unused int at91_adc_resume(struct device *dev)
 		return 0;
 
 	/* check if we are enabling triggered buffer or the touchscreen */
-	if (bitmap_subset(indio_dev->active_scan_mask,
-			  &st->touch_st.channels_bitmask,
-			  AT91_SAMA5D2_MAX_CHAN_IDX + 1)) {
-		/* touchscreen enabling */
+	if (at91_adc_current_chan_is_touch(indio_dev))
 		return at91_adc_configure_touch(st, true);
-	} else {
+	else
 		return at91_adc_configure_trigger(st->trig, true);
-	}
 
 	/* not needed but more explicit */
 	return 0;
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index abe99856c823..0368b6dc6d60 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -1152,7 +1152,6 @@ static int at91_adc_probe(struct platform_device *pdev)
 	int ret;
 	struct iio_dev *idev;
 	struct at91_adc_state *st;
-	struct resource *res;
 	u32 reg;
 
 	idev = devm_iio_device_alloc(&pdev->dev, sizeof(struct at91_adc_state));
@@ -1182,9 +1181,7 @@ static int at91_adc_probe(struct platform_device *pdev)
 	if (st->irq < 0)
 		return -ENODEV;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	st->reg_base = devm_ioremap_resource(&pdev->dev, res);
+	st->reg_base = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(st->reg_base))
 		return PTR_ERR(st->reg_base);
 
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index 22131a677445..6bda4f4d89fe 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -449,9 +449,6 @@ static void exynos_adc_exynos7_init_hw(struct exynos_adc *info)
 {
 	u32 con1, con2;
 
-	if (info->data->needs_adc_phy)
-		regmap_write(info->pmu_map, info->data->phy_offset, 1);
-
 	con1 = ADC_V2_CON1_SOFT_RESET;
 	writel(con1, ADC_V2_CON1(info->regs));
 
@@ -531,8 +528,19 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 	unsigned long timeout;
 	int ret;
 
-	if (mask != IIO_CHAN_INFO_RAW)
+	if (mask == IIO_CHAN_INFO_SCALE) {
+		ret = regulator_get_voltage(info->vdd);
+		if (ret < 0)
+			return ret;
+
+		/* Regulator voltage is in uV, but need mV */
+		*val = ret / 1000;
+		*val2 = info->data->mask;
+
+		return IIO_VAL_FRACTIONAL;
+	} else if (mask != IIO_CHAN_INFO_RAW) {
 		return -EINVAL;
+	}
 
 	mutex_lock(&indio_dev->mlock);
 	reinit_completion(&info->completion);
@@ -683,6 +691,7 @@ static const struct iio_info exynos_adc_iio_info = {
 	.channel = _index,				\
 	.address = _index,				\
 	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),	\
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SCALE),	\
 	.datasheet_name = _id,				\
 }
 
diff --git a/drivers/iio/adc/fsl-imx25-gcq.c b/drivers/iio/adc/fsl-imx25-gcq.c
index fa71489195c6..b0a4dc88ba9b 100644
--- a/drivers/iio/adc/fsl-imx25-gcq.c
+++ b/drivers/iio/adc/fsl-imx25-gcq.c
@@ -294,7 +294,6 @@ static int mx25_gcq_probe(struct platform_device *pdev)
 	struct mx25_gcq_priv *priv;
 	struct mx25_tsadc *tsadc = dev_get_drvdata(pdev->dev.parent);
 	struct device *dev = &pdev->dev;
-	struct resource *res;
 	void __iomem *mem;
 	int ret;
 	int i;
@@ -305,8 +304,7 @@ static int mx25_gcq_probe(struct platform_device *pdev)
 
 	priv = iio_priv(indio_dev);
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	mem = devm_ioremap_resource(dev, res);
+	mem = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(mem))
 		return PTR_ERR(mem);
 
diff --git a/drivers/iio/adc/intel_mrfld_adc.c b/drivers/iio/adc/intel_mrfld_adc.c
index c35a1beb817c..a6d2e1f27e76 100644
--- a/drivers/iio/adc/intel_mrfld_adc.c
+++ b/drivers/iio/adc/intel_mrfld_adc.c
@@ -75,7 +75,7 @@ static int mrfld_adc_single_conv(struct iio_dev *indio_dev,
 	struct regmap *regmap = adc->regmap;
 	unsigned int req;
 	long timeout;
-	u8 buf[2];
+	__be16 value;
 	int ret;
 
 	reinit_completion(&adc->completion);
@@ -105,11 +105,11 @@ static int mrfld_adc_single_conv(struct iio_dev *indio_dev,
 		goto done;
 	}
 
-	ret = regmap_bulk_read(regmap, chan->address, buf, 2);
+	ret = regmap_bulk_read(regmap, chan->address, &value, sizeof(value));
 	if (ret)
 		goto done;
 
-	*result = get_unaligned_be16(buf);
+	*result = be16_to_cpu(value);
 	ret = IIO_VAL_INT;
 
 done:
diff --git a/drivers/iio/adc/max1241.c b/drivers/iio/adc/max1241.c
new file mode 100644
index 000000000000..541939c7abca
--- /dev/null
+++ b/drivers/iio/adc/max1241.c
@@ -0,0 +1,227 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * MAX1241 low-power, 12-bit serial ADC
+ *
+ * Datasheet: https://datasheets.maximintegrated.com/en/ds/MAX1240-MAX1241.pdf
+ */
+
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/iio/iio.h>
+#include <linux/module.h>
+#include <linux/regulator/consumer.h>
+#include <linux/spi/spi.h>
+
+#define MAX1241_VAL_MASK GENMASK(11, 0)
+#define MAX1241_SHUTDOWN_DELAY_USEC 4
+
+enum max1241_id {
+	max1241,
+};
+
+struct max1241 {
+	struct spi_device *spi;
+	struct mutex lock;
+	struct regulator *vdd;
+	struct regulator *vref;
+	struct gpio_desc *shutdown;
+
+	__be16 data ____cacheline_aligned;
+};
+
+static const struct iio_chan_spec max1241_channels[] = {
+	{
+		.type = IIO_VOLTAGE,
+		.indexed = 1,
+		.channel = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				BIT(IIO_CHAN_INFO_SCALE),
+	},
+};
+
+static int max1241_read(struct max1241 *adc)
+{
+	struct spi_transfer xfers[] = {
+		/*
+		 * Begin conversion by bringing /CS low for at least
+		 * tconv us.
+		 */
+		{
+			.len = 0,
+			.delay.value = 8,
+			.delay.unit = SPI_DELAY_UNIT_USECS,
+		},
+		/*
+		 * Then read two bytes of data in our RX buffer.
+		 */
+		{
+			.rx_buf = &adc->data,
+			.len = 2,
+		},
+	};
+
+	return spi_sync_transfer(adc->spi, xfers, ARRAY_SIZE(xfers));
+}
+
+static int max1241_read_raw(struct iio_dev *indio_dev,
+			struct iio_chan_spec const *chan,
+			int *val, int *val2, long mask)
+{
+	int ret, vref_uV;
+	struct max1241 *adc = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&adc->lock);
+
+		if (adc->shutdown) {
+			gpiod_set_value(adc->shutdown, 0);
+			udelay(MAX1241_SHUTDOWN_DELAY_USEC);
+			ret = max1241_read(adc);
+			gpiod_set_value(adc->shutdown, 1);
+		} else
+			ret = max1241_read(adc);
+
+		if (ret) {
+			mutex_unlock(&adc->lock);
+			return ret;
+		}
+
+		*val = (be16_to_cpu(adc->data) >> 3) & MAX1241_VAL_MASK;
+
+		mutex_unlock(&adc->lock);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		vref_uV = regulator_get_voltage(adc->vref);
+
+		if (vref_uV < 0)
+			return vref_uV;
+
+		*val = vref_uV / 1000;
+		*val2 = 12;
+
+		return IIO_VAL_FRACTIONAL_LOG2;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info max1241_info = {
+	.read_raw = max1241_read_raw,
+};
+
+static void max1241_disable_vdd_action(void *data)
+{
+	struct max1241 *adc = data;
+	struct device *dev = &adc->spi->dev;
+	int err;
+
+	err = regulator_disable(adc->vdd);
+	if (err)
+		dev_err(dev, "could not disable vdd regulator.\n");
+}
+
+static void max1241_disable_vref_action(void *data)
+{
+	struct max1241 *adc = data;
+	struct device *dev = &adc->spi->dev;
+	int err;
+
+	err = regulator_disable(adc->vref);
+	if (err)
+		dev_err(dev, "could not disable vref regulator.\n");
+}
+
+static int max1241_probe(struct spi_device *spi)
+{
+	struct device *dev = &spi->dev;
+	struct iio_dev *indio_dev;
+	struct max1241 *adc;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc = iio_priv(indio_dev);
+	adc->spi = spi;
+	mutex_init(&adc->lock);
+
+	spi_set_drvdata(spi, indio_dev);
+
+	adc->vdd = devm_regulator_get(dev, "vdd");
+	if (IS_ERR(adc->vdd)) {
+		dev_err(dev, "failed to get vdd regulator\n");
+		return PTR_ERR(adc->vdd);
+	}
+
+	ret = regulator_enable(adc->vdd);
+	if (ret)
+		return ret;
+
+	ret = devm_add_action_or_reset(dev, max1241_disable_vdd_action, adc);
+	if (ret) {
+		dev_err(dev, "could not set up vdd regulator cleanup action\n");
+		return ret;
+	}
+
+	adc->vref = devm_regulator_get(dev, "vref");
+	if (IS_ERR(adc->vref)) {
+		dev_err(dev, "failed to get vref regulator\n");
+		return PTR_ERR(adc->vref);
+	}
+
+	ret = regulator_enable(adc->vref);
+	if (ret)
+		return ret;
+
+	ret = devm_add_action_or_reset(dev, max1241_disable_vref_action, adc);
+	if (ret) {
+		dev_err(dev, "could not set up vref regulator cleanup action\n");
+		return ret;
+	}
+
+	adc->shutdown = devm_gpiod_get_optional(dev, "shutdown",
+						GPIOD_OUT_HIGH);
+	if (IS_ERR(adc->shutdown))
+		return PTR_ERR(adc->shutdown);
+
+	if (adc->shutdown)
+		dev_dbg(dev, "shutdown pin passed, low-power mode enabled");
+	else
+		dev_dbg(dev, "no shutdown pin passed, low-power mode disabled");
+
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->dev.parent = dev;
+	indio_dev->info = &max1241_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->channels = max1241_channels;
+	indio_dev->num_channels = ARRAY_SIZE(max1241_channels);
+
+	return devm_iio_device_register(dev, indio_dev);
+}
+
+static const struct spi_device_id max1241_id[] = {
+	{ "max1241", max1241 },
+	{}
+};
+
+static const struct of_device_id max1241_dt_ids[] = {
+	{ .compatible = "maxim,max1241" },
+	{}
+};
+MODULE_DEVICE_TABLE(of, max1241_dt_ids);
+
+static struct spi_driver max1241_spi_driver = {
+	.driver = {
+		.name = "max1241",
+		.of_match_table = max1241_dt_ids,
+	},
+	.probe = max1241_probe,
+	.id_table = max1241_id,
+};
+module_spi_driver(max1241_spi_driver);
+
+MODULE_AUTHOR("Alexandru Lazar <alazar@startmail.com>");
+MODULE_DESCRIPTION("MAX1241 ADC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/max1363.c b/drivers/iio/adc/max1363.c
index 5c2cc61b666e..9d92017c79b2 100644
--- a/drivers/iio/adc/max1363.c
+++ b/drivers/iio/adc/max1363.c
@@ -150,6 +150,7 @@ struct max1363_chip_info {
  * @current_mode:	the scan mode of this chip
  * @requestedmask:	a valid requested set of channels
  * @reg:		supply regulator
+ * @lock		lock to ensure state is consistent
  * @monitor_on:		whether monitor mode is enabled
  * @monitor_speed:	parameter corresponding to device monitor speed setting
  * @mask_high:		bitmask for enabled high thresholds
@@ -169,6 +170,7 @@ struct max1363_state {
 	const struct max1363_mode	*current_mode;
 	u32				requestedmask;
 	struct regulator		*reg;
+	struct mutex			lock;
 
 	/* Using monitor modes and buffer at the same time is
 	   currently not supported */
@@ -364,7 +366,11 @@ static int max1363_read_single_chan(struct iio_dev *indio_dev,
 	struct max1363_state *st = iio_priv(indio_dev);
 	struct i2c_client *client = st->client;
 
-	mutex_lock(&indio_dev->mlock);
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+	mutex_lock(&st->lock);
+
 	/*
 	 * If monitor mode is enabled, the method for reading a single
 	 * channel will have to be rather different and has not yet
@@ -372,7 +378,7 @@ static int max1363_read_single_chan(struct iio_dev *indio_dev,
 	 *
 	 * Also, cannot read directly if buffered capture enabled.
 	 */
-	if (st->monitor_on || iio_buffer_enabled(indio_dev)) {
+	if (st->monitor_on) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
@@ -404,8 +410,10 @@ static int max1363_read_single_chan(struct iio_dev *indio_dev,
 		data = rxbuf[0];
 	}
 	*val = data;
+
 error_ret:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
+	iio_device_release_direct_mode(indio_dev);
 	return ret;
 
 }
@@ -705,9 +713,9 @@ static ssize_t max1363_monitor_store_freq(struct device *dev,
 	if (!found)
 		return -EINVAL;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	st->monitor_speed = i;
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return 0;
 }
@@ -810,12 +818,12 @@ static int max1363_read_event_config(struct iio_dev *indio_dev,
 	int val;
 	int number = chan->channel;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	if (dir == IIO_EV_DIR_FALLING)
 		val = (1 << number) & st->mask_low;
 	else
 		val = (1 << number) & st->mask_high;
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return val;
 }
@@ -962,7 +970,11 @@ static int max1363_write_event_config(struct iio_dev *indio_dev,
 	u16 unifiedmask;
 	int number = chan->channel;
 
-	mutex_lock(&indio_dev->mlock);
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+	mutex_lock(&st->lock);
+
 	unifiedmask = st->mask_low | st->mask_high;
 	if (dir == IIO_EV_DIR_FALLING) {
 
@@ -989,7 +1001,8 @@ static int max1363_write_event_config(struct iio_dev *indio_dev,
 
 	max1363_monitor_mode_update(st, !!(st->mask_high | st->mask_low));
 error_ret:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
+	iio_device_release_direct_mode(indio_dev);
 
 	return ret;
 }
@@ -1587,6 +1600,7 @@ static int max1363_probe(struct i2c_client *client,
 
 	st = iio_priv(indio_dev);
 
+	mutex_init(&st->lock);
 	st->reg = devm_regulator_get(&client->dev, "vcc");
 	if (IS_ERR(st->reg)) {
 		ret = PTR_ERR(st->reg);
diff --git a/drivers/iio/adc/mcp3422.c b/drivers/iio/adc/mcp3422.c
index ea24d7c58b12..d86c0b5d80a3 100644
--- a/drivers/iio/adc/mcp3422.c
+++ b/drivers/iio/adc/mcp3422.c
@@ -19,6 +19,7 @@
 #include <linux/delay.h>
 #include <linux/sysfs.h>
 #include <linux/of.h>
+#include <asm/unaligned.h>
 
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -117,11 +118,11 @@ static int mcp3422_read(struct mcp3422 *adc, int *value, u8 *config)
 
 	if (sample_rate == MCP3422_SRATE_3) {
 		ret = i2c_master_recv(adc->i2c, buf, 4);
-		temp = buf[0] << 16 | buf[1] << 8 | buf[2];
+		temp = get_unaligned_be24(&buf[0]);
 		*config = buf[3];
 	} else {
 		ret = i2c_master_recv(adc->i2c, buf, 3);
-		temp = buf[0] << 8 | buf[1];
+		temp = get_unaligned_be16(&buf[0]);
 		*config = buf[2];
 	}
 
diff --git a/drivers/iio/adc/stm32-adc-core.c b/drivers/iio/adc/stm32-adc-core.c
index 2df88d2b880a..0e2068ec068b 100644
--- a/drivers/iio/adc/stm32-adc-core.c
+++ b/drivers/iio/adc/stm32-adc-core.c
@@ -65,12 +65,14 @@ struct stm32_adc_priv;
  * @clk_sel:	clock selection routine
  * @max_clk_rate_hz: maximum analog clock rate (Hz, from datasheet)
  * @has_syscfg: SYSCFG capability flags
+ * @num_irqs:	number of interrupt lines
  */
 struct stm32_adc_priv_cfg {
 	const struct stm32_adc_common_regs *regs;
 	int (*clk_sel)(struct platform_device *, struct stm32_adc_priv *);
 	u32 max_clk_rate_hz;
 	unsigned int has_syscfg;
+	unsigned int num_irqs;
 };
 
 /**
@@ -375,21 +377,15 @@ static int stm32_adc_irq_probe(struct platform_device *pdev,
 	struct device_node *np = pdev->dev.of_node;
 	unsigned int i;
 
-	for (i = 0; i < STM32_ADC_MAX_ADCS; i++) {
+	/*
+	 * Interrupt(s) must be provided, depending on the compatible:
+	 * - stm32f4/h7 shares a common interrupt line.
+	 * - stm32mp1, has one line per ADC
+	 */
+	for (i = 0; i < priv->cfg->num_irqs; i++) {
 		priv->irq[i] = platform_get_irq(pdev, i);
-		if (priv->irq[i] < 0) {
-			/*
-			 * At least one interrupt must be provided, make others
-			 * optional:
-			 * - stm32f4/h7 shares a common interrupt.
-			 * - stm32mp1, has one line per ADC (either for ADC1,
-			 *   ADC2 or both).
-			 */
-			if (i && priv->irq[i] == -ENXIO)
-				continue;
-
+		if (priv->irq[i] < 0)
 			return priv->irq[i];
-		}
 	}
 
 	priv->domain = irq_domain_add_simple(np, STM32_ADC_MAX_ADCS, 0,
@@ -400,9 +396,7 @@ static int stm32_adc_irq_probe(struct platform_device *pdev,
 		return -ENOMEM;
 	}
 
-	for (i = 0; i < STM32_ADC_MAX_ADCS; i++) {
-		if (priv->irq[i] < 0)
-			continue;
+	for (i = 0; i < priv->cfg->num_irqs; i++) {
 		irq_set_chained_handler(priv->irq[i], stm32_adc_irq_handler);
 		irq_set_handler_data(priv->irq[i], priv);
 	}
@@ -420,11 +414,8 @@ static void stm32_adc_irq_remove(struct platform_device *pdev,
 		irq_dispose_mapping(irq_find_mapping(priv->domain, hwirq));
 	irq_domain_remove(priv->domain);
 
-	for (i = 0; i < STM32_ADC_MAX_ADCS; i++) {
-		if (priv->irq[i] < 0)
-			continue;
+	for (i = 0; i < priv->cfg->num_irqs; i++)
 		irq_set_chained_handler(priv->irq[i], NULL);
-	}
 }
 
 static int stm32_adc_core_switches_supply_en(struct stm32_adc_priv *priv,
@@ -817,6 +808,7 @@ static const struct stm32_adc_priv_cfg stm32f4_adc_priv_cfg = {
 	.regs = &stm32f4_adc_common_regs,
 	.clk_sel = stm32f4_adc_clk_sel,
 	.max_clk_rate_hz = 36000000,
+	.num_irqs = 1,
 };
 
 static const struct stm32_adc_priv_cfg stm32h7_adc_priv_cfg = {
@@ -824,6 +816,7 @@ static const struct stm32_adc_priv_cfg stm32h7_adc_priv_cfg = {
 	.clk_sel = stm32h7_adc_clk_sel,
 	.max_clk_rate_hz = 36000000,
 	.has_syscfg = HAS_VBOOSTER,
+	.num_irqs = 1,
 };
 
 static const struct stm32_adc_priv_cfg stm32mp1_adc_priv_cfg = {
@@ -831,6 +824,7 @@ static const struct stm32_adc_priv_cfg stm32mp1_adc_priv_cfg = {
 	.clk_sel = stm32h7_adc_clk_sel,
 	.max_clk_rate_hz = 40000000,
 	.has_syscfg = HAS_VBOOSTER | HAS_ANASWVDD,
+	.num_irqs = 2,
 };
 
 static const struct of_device_id stm32_adc_of_match[] = {
diff --git a/drivers/iio/adc/sun4i-gpadc-iio.c b/drivers/iio/adc/sun4i-gpadc-iio.c
index 176e1cb4abb1..0f2c1738a90d 100644
--- a/drivers/iio/adc/sun4i-gpadc-iio.c
+++ b/drivers/iio/adc/sun4i-gpadc-iio.c
@@ -496,7 +496,6 @@ static int sun4i_gpadc_probe_dt(struct platform_device *pdev,
 				struct iio_dev *indio_dev)
 {
 	struct sun4i_gpadc_iio *info = iio_priv(indio_dev);
-	struct resource *mem;
 	void __iomem *base;
 	int ret;
 
@@ -508,8 +507,7 @@ static int sun4i_gpadc_probe_dt(struct platform_device *pdev,
 	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);
+	base = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
diff --git a/drivers/iio/adc/ti-ads124s08.c b/drivers/iio/adc/ti-ads124s08.c
index 552c2be8d87a..f1ee3b1e2827 100644
--- a/drivers/iio/adc/ti-ads124s08.c
+++ b/drivers/iio/adc/ti-ads124s08.c
@@ -22,6 +22,8 @@
 #include <linux/iio/triggered_buffer.h>
 #include <linux/iio/sysfs.h>
 
+#include <asm/unaligned.h>
+
 /* Commands */
 #define ADS124S08_CMD_NOP	0x00
 #define ADS124S08_CMD_WAKEUP	0x02
@@ -188,7 +190,6 @@ static int ads124s_read(struct iio_dev *indio_dev, unsigned int chan)
 {
 	struct ads124s_private *priv = iio_priv(indio_dev);
 	int ret;
-	u32 tmp;
 	struct spi_transfer t[] = {
 		{
 			.tx_buf = &priv->data[0],
@@ -208,9 +209,7 @@ static int ads124s_read(struct iio_dev *indio_dev, unsigned int chan)
 	if (ret < 0)
 		return ret;
 
-	tmp = priv->data[2] << 16 | priv->data[3] << 8 | priv->data[4];
-
-	return tmp;
+	return get_unaligned_be24(&priv->data[2]);
 }
 
 static int ads124s_read_raw(struct iio_dev *indio_dev,
diff --git a/drivers/iio/adc/xilinx-xadc-core.c b/drivers/iio/adc/xilinx-xadc-core.c
index 6fd06e4eff73..d7fecab9252e 100644
--- a/drivers/iio/adc/xilinx-xadc-core.c
+++ b/drivers/iio/adc/xilinx-xadc-core.c
@@ -3,7 +3,7 @@
  * Xilinx XADC driver
  *
  * Copyright 2013-2014 Analog Devices Inc.
- *  Author: Lars-Peter Clauen <lars@metafoo.de>
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
  *
  * Documentation for the parts can be found at:
  *  - XADC hardmacro: Xilinx UG480
@@ -663,7 +663,7 @@ static int xadc_trigger_set_state(struct iio_trigger *trigger, bool state)
 	mutex_lock(&xadc->mutex);
 
 	if (state) {
-		/* Only one of the two triggers can be active at the a time. */
+		/* Only one of the two triggers can be active at a time. */
 		if (xadc->trigger != NULL) {
 			ret = -EBUSY;
 			goto err_out;
diff --git a/drivers/iio/adc/xilinx-xadc-events.c b/drivers/iio/adc/xilinx-xadc-events.c
index dbfd5da290a4..2357f585720a 100644
--- a/drivers/iio/adc/xilinx-xadc-events.c
+++ b/drivers/iio/adc/xilinx-xadc-events.c
@@ -3,7 +3,7 @@
  * Xilinx XADC driver
  *
  * Copyright 2013 Analog Devices Inc.
- *  Author: Lars-Peter Clauen <lars@metafoo.de>
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
  */
 
 #include <linux/iio/events.h>
diff --git a/drivers/iio/adc/xilinx-xadc.h b/drivers/iio/adc/xilinx-xadc.h
index 4017f18b0a4f..25abed9c0285 100644
--- a/drivers/iio/adc/xilinx-xadc.h
+++ b/drivers/iio/adc/xilinx-xadc.h
@@ -3,7 +3,7 @@
  * Xilinx XADC driver
  *
  * Copyright 2013 Analog Devices Inc.
- *  Author: Lars-Peter Clauen <lars@metafoo.de>
+ *  Author: Lars-Peter Clausen <lars@metafoo.de>
  */
 
 #ifndef __IIO_XILINX_XADC__
diff --git a/drivers/iio/buffer/industrialio-buffer-dma.c b/drivers/iio/buffer/industrialio-buffer-dma.c
index a74bd9c0587c..d348af8b9705 100644
--- a/drivers/iio/buffer/industrialio-buffer-dma.c
+++ b/drivers/iio/buffer/industrialio-buffer-dma.c
@@ -12,7 +12,6 @@
 #include <linux/mutex.h>
 #include <linux/sched.h>
 #include <linux/poll.h>
-#include <linux/iio/buffer.h>
 #include <linux/iio/buffer_impl.h>
 #include <linux/iio/buffer-dma.h>
 #include <linux/dma-mapping.h>
diff --git a/drivers/iio/buffer/industrialio-buffer-dmaengine.c b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
index b129693af0fd..6dedf12b69a4 100644
--- a/drivers/iio/buffer/industrialio-buffer-dmaengine.c
+++ b/drivers/iio/buffer/industrialio-buffer-dmaengine.c
@@ -134,7 +134,7 @@ static ssize_t iio_dmaengine_buffer_get_length_align(struct device *dev,
 	struct dmaengine_buffer *dmaengine_buffer =
 		iio_buffer_to_dmaengine_buffer(indio_dev->buffer);
 
-	return sprintf(buf, "%u\n", dmaengine_buffer->align);
+	return sprintf(buf, "%zu\n", dmaengine_buffer->align);
 }
 
 static IIO_DEVICE_ATTR(length_align_bytes, 0444,
@@ -229,6 +229,45 @@ void iio_dmaengine_buffer_free(struct iio_buffer *buffer)
 }
 EXPORT_SYMBOL_GPL(iio_dmaengine_buffer_free);
 
+static void __devm_iio_dmaengine_buffer_free(struct device *dev, void *res)
+{
+	iio_dmaengine_buffer_free(*(struct iio_buffer **)res);
+}
+
+/**
+ * devm_iio_dmaengine_buffer_alloc() - Resource-managed iio_dmaengine_buffer_alloc()
+ * @dev: Parent device for the buffer
+ * @channel: DMA channel name, typically "rx".
+ *
+ * This allocates a new IIO buffer which internally uses the DMAengine framework
+ * to perform its transfers. The parent device will be used to request the DMA
+ * channel.
+ *
+ * The buffer will be automatically de-allocated once the device gets destroyed.
+ */
+struct iio_buffer *devm_iio_dmaengine_buffer_alloc(struct device *dev,
+	const char *channel)
+{
+	struct iio_buffer **bufferp, *buffer;
+
+	bufferp = devres_alloc(__devm_iio_dmaengine_buffer_free,
+			       sizeof(*bufferp), GFP_KERNEL);
+	if (!bufferp)
+		return ERR_PTR(-ENOMEM);
+
+	buffer = iio_dmaengine_buffer_alloc(dev, channel);
+	if (IS_ERR(buffer)) {
+		devres_free(bufferp);
+		return buffer;
+	}
+
+	*bufferp = buffer;
+	devres_add(dev, bufferp);
+
+	return buffer;
+}
+EXPORT_SYMBOL_GPL(devm_iio_dmaengine_buffer_alloc);
+
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_DESCRIPTION("DMA buffer for the IIO framework");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/buffer/industrialio-hw-consumer.c b/drivers/iio/buffer/industrialio-hw-consumer.c
index 95165697d8ae..f2d27788f666 100644
--- a/drivers/iio/buffer/industrialio-hw-consumer.c
+++ b/drivers/iio/buffer/industrialio-hw-consumer.c
@@ -142,17 +142,6 @@ static void devm_iio_hw_consumer_release(struct device *dev, void *res)
 	iio_hw_consumer_free(*(struct iio_hw_consumer **)res);
 }
 
-static int devm_iio_hw_consumer_match(struct device *dev, void *res, void *data)
-{
-	struct iio_hw_consumer **r = res;
-
-	if (!r || !*r) {
-		WARN_ON(!r || !*r);
-		return 0;
-	}
-	return *r == data;
-}
-
 /**
  * devm_iio_hw_consumer_alloc - Resource-managed iio_hw_consumer_alloc()
  * @dev: Pointer to consumer device.
@@ -160,9 +149,6 @@ static int devm_iio_hw_consumer_match(struct device *dev, void *res, void *data)
  * Managed iio_hw_consumer_alloc. iio_hw_consumer allocated with this function
  * is automatically freed on driver detach.
  *
- * If an iio_hw_consumer allocated with this function needs to be freed
- * separately, devm_iio_hw_consumer_free() must be used.
- *
  * returns pointer to allocated iio_hw_consumer on success, NULL on failure.
  */
 struct iio_hw_consumer *devm_iio_hw_consumer_alloc(struct device *dev)
@@ -187,23 +173,6 @@ struct iio_hw_consumer *devm_iio_hw_consumer_alloc(struct device *dev)
 EXPORT_SYMBOL_GPL(devm_iio_hw_consumer_alloc);
 
 /**
- * devm_iio_hw_consumer_free - Resource-managed iio_hw_consumer_free()
- * @dev: Pointer to consumer device.
- * @hwc: iio_hw_consumer to free.
- *
- * Free iio_hw_consumer allocated with devm_iio_hw_consumer_alloc().
- */
-void devm_iio_hw_consumer_free(struct device *dev, struct iio_hw_consumer *hwc)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_hw_consumer_release,
-			    devm_iio_hw_consumer_match, hwc);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_hw_consumer_free);
-
-/**
  * iio_hw_consumer_enable() - Enable IIO hardware consumer
  * @hwc: iio_hw_consumer to enable.
  *
diff --git a/drivers/iio/buffer/industrialio-triggered-buffer.c b/drivers/iio/buffer/industrialio-triggered-buffer.c
index cb322b2f09cd..e8046c1ecd6b 100644
--- a/drivers/iio/buffer/industrialio-triggered-buffer.c
+++ b/drivers/iio/buffer/industrialio-triggered-buffer.c
@@ -126,17 +126,6 @@ int devm_iio_triggered_buffer_setup(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(devm_iio_triggered_buffer_setup);
 
-void devm_iio_triggered_buffer_cleanup(struct device *dev,
-				       struct iio_dev *indio_dev)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_triggered_buffer_clean,
-			    devm_iio_device_match, indio_dev);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_triggered_buffer_cleanup);
-
 MODULE_AUTHOR("Lars-Peter Clausen <lars@metafoo.de>");
 MODULE_DESCRIPTION("IIO helper functions for setting up triggered buffers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/buffer/kfifo_buf.c b/drivers/iio/buffer/kfifo_buf.c
index 3150f8ab984b..1359abed3b31 100644
--- a/drivers/iio/buffer/kfifo_buf.c
+++ b/drivers/iio/buffer/kfifo_buf.c
@@ -179,16 +179,6 @@ static void devm_iio_kfifo_release(struct device *dev, void *res)
 	iio_kfifo_free(*(struct iio_buffer **)res);
 }
 
-static int devm_iio_kfifo_match(struct device *dev, void *res, void *data)
-{
-	struct iio_buffer **r = res;
-
-	if (WARN_ON(!r || !*r))
-		return 0;
-
-	return *r == data;
-}
-
 /**
  * devm_iio_fifo_allocate - Resource-managed iio_kfifo_allocate()
  * @dev:		Device to allocate kfifo buffer for
@@ -216,16 +206,4 @@ struct iio_buffer *devm_iio_kfifo_allocate(struct device *dev)
 }
 EXPORT_SYMBOL(devm_iio_kfifo_allocate);
 
-/**
- * devm_iio_fifo_free - Resource-managed iio_kfifo_free()
- * @dev:		Device the buffer belongs to
- * @r:			The buffer associated with the device
- */
-void devm_iio_kfifo_free(struct device *dev, struct iio_buffer *r)
-{
-	WARN_ON(devres_release(dev, devm_iio_kfifo_release,
-			       devm_iio_kfifo_match, r));
-}
-EXPORT_SYMBOL(devm_iio_kfifo_free);
-
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/chemical/Kconfig b/drivers/iio/chemical/Kconfig
index a7e65a59bf42..7f21afd73b1c 100644
--- a/drivers/iio/chemical/Kconfig
+++ b/drivers/iio/chemical/Kconfig
@@ -22,6 +22,17 @@ config ATLAS_PH_SENSOR
 	  To compile this driver as module, choose M here: the
 	  module will be called atlas-ph-sensor.
 
+config ATLAS_EZO_SENSOR
+	tristate "Atlas Scientific EZO sensors"
+	depends on I2C
+	help
+	  Say Y here to build I2C interface support for the following
+	  Atlas Scientific EZO sensors
+	    * CO2 EZO Sensor
+
+	  To compile this driver as module, choose M here: the
+	  module will be called atlas-ezo-sensor.
+
 config BME680
 	tristate "Bosch Sensortec BME680 sensor driver"
 	depends on (I2C || SPI)
diff --git a/drivers/iio/chemical/Makefile b/drivers/iio/chemical/Makefile
index 33d3a595dda9..aba4167db745 100644
--- a/drivers/iio/chemical/Makefile
+++ b/drivers/iio/chemical/Makefile
@@ -5,6 +5,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_ATLAS_PH_SENSOR)	+= atlas-sensor.o
+obj-$(CONFIG_ATLAS_EZO_SENSOR)	+= atlas-ezo-sensor.o
 obj-$(CONFIG_BME680) += bme680_core.o
 obj-$(CONFIG_BME680_I2C) += bme680_i2c.o
 obj-$(CONFIG_BME680_SPI) += bme680_spi.o
diff --git a/drivers/iio/chemical/atlas-ezo-sensor.c b/drivers/iio/chemical/atlas-ezo-sensor.c
new file mode 100644
index 000000000000..f5a6d8ec6d4d
--- /dev/null
+++ b/drivers/iio/chemical/atlas-ezo-sensor.c
@@ -0,0 +1,177 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * atlas-ezo-sensor.c - Support for Atlas Scientific EZO sensors
+ *
+ * Copyright (C) 2020 Konsulko Group
+ * Author: Matt Ranostay <matt.ranostay@konsulko.com>
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/of_device.h>
+#include <linux/iio/iio.h>
+
+#define ATLAS_EZO_DRV_NAME		"atlas-ezo-sensor"
+#define ATLAS_CO2_INT_TIME_IN_MS	950
+
+enum {
+	ATLAS_CO2_EZO,
+};
+
+struct atlas_ezo_device {
+	const struct iio_chan_spec *channels;
+	int num_channels;
+	int delay;
+};
+
+struct atlas_ezo_data {
+	struct i2c_client *client;
+	struct atlas_ezo_device *chip;
+
+	/* lock to avoid multiple concurrent read calls */
+	struct mutex lock;
+
+	u8 buffer[8];
+};
+
+static const struct iio_chan_spec atlas_co2_ezo_channels[] = {
+	{
+		.type = IIO_CONCENTRATION,
+		.modified = 1,
+		.channel2 = IIO_MOD_CO2,
+		.info_mask_separate =
+			BIT(IIO_CHAN_INFO_RAW) | BIT(IIO_CHAN_INFO_SCALE),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 32,
+			.storagebits = 32,
+			.endianness = IIO_CPU,
+		},
+	},
+};
+
+static struct atlas_ezo_device atlas_ezo_devices[] = {
+	[ATLAS_CO2_EZO] = {
+		.channels = atlas_co2_ezo_channels,
+		.num_channels = 1,
+		.delay = ATLAS_CO2_INT_TIME_IN_MS,
+	},
+};
+
+static int atlas_ezo_read_raw(struct iio_dev *indio_dev,
+			  struct iio_chan_spec const *chan,
+			  int *val, int *val2, long mask)
+{
+	struct atlas_ezo_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	int ret = 0;
+
+	if (chan->type != IIO_CONCENTRATION)
+		return -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW: {
+		long tmp;
+
+		mutex_lock(&data->lock);
+
+		tmp = i2c_smbus_write_byte(client, 'R');
+
+		if (tmp < 0) {
+			mutex_unlock(&data->lock);
+			return tmp;
+		}
+
+		msleep(data->chip->delay);
+
+		tmp = i2c_master_recv(client, data->buffer, sizeof(data->buffer));
+
+		if (tmp < 0 || data->buffer[0] != 1) {
+			mutex_unlock(&data->lock);
+			return -EBUSY;
+		}
+
+		ret = kstrtol(data->buffer + 1, 10, &tmp);
+
+		*val = tmp;
+
+		mutex_unlock(&data->lock);
+
+		return ret ? ret : IIO_VAL_INT;
+	}
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		*val2 = 100; /* 0.0001 */
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+
+	return ret;
+}
+
+static const struct iio_info atlas_info = {
+	.read_raw = atlas_ezo_read_raw,
+};
+
+static const struct i2c_device_id atlas_ezo_id[] = {
+	{ "atlas-co2-ezo", ATLAS_CO2_EZO },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, atlas_ezo_id);
+
+static const struct of_device_id atlas_ezo_dt_ids[] = {
+	{ .compatible = "atlas,co2-ezo", .data = (void *)ATLAS_CO2_EZO, },
+	{}
+};
+MODULE_DEVICE_TABLE(of, atlas_ezo_dt_ids);
+
+static int atlas_ezo_probe(struct i2c_client *client,
+		       const struct i2c_device_id *id)
+{
+	struct atlas_ezo_data *data;
+	struct atlas_ezo_device *chip;
+	const struct of_device_id *of_id;
+	struct iio_dev *indio_dev;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	of_id = of_match_device(atlas_ezo_dt_ids, &client->dev);
+	if (!of_id)
+		chip = &atlas_ezo_devices[id->driver_data];
+	else
+		chip = &atlas_ezo_devices[(unsigned long)of_id->data];
+
+	indio_dev->info = &atlas_info;
+	indio_dev->name = ATLAS_EZO_DRV_NAME;
+	indio_dev->channels = chip->channels;
+	indio_dev->num_channels = chip->num_channels;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->dev.parent = &client->dev;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	data->chip = chip;
+	mutex_init(&data->lock);
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+};
+
+static struct i2c_driver atlas_ezo_driver = {
+	.driver = {
+		.name	= ATLAS_EZO_DRV_NAME,
+		.of_match_table	= atlas_ezo_dt_ids,
+	},
+	.probe		= atlas_ezo_probe,
+	.id_table	= atlas_ezo_id,
+};
+module_i2c_driver(atlas_ezo_driver);
+
+MODULE_AUTHOR("Matt Ranostay <matt.ranostay@konsulko.com>");
+MODULE_DESCRIPTION("Atlas Scientific EZO sensors");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/chemical/atlas-sensor.c b/drivers/iio/chemical/atlas-sensor.c
index 7b199ce16ecf..78a27e36bf32 100644
--- a/drivers/iio/chemical/atlas-sensor.c
+++ b/drivers/iio/chemical/atlas-sensor.c
@@ -53,6 +53,8 @@
 #define ATLAS_REG_DO_CALIB_STATUS_PRESSURE	BIT(0)
 #define ATLAS_REG_DO_CALIB_STATUS_DO		BIT(1)
 
+#define ATLAS_REG_RTD_DATA		0x0e
+
 #define ATLAS_REG_PH_TEMP_DATA		0x0e
 #define ATLAS_REG_PH_DATA		0x16
 
@@ -72,12 +74,14 @@
 #define ATLAS_EC_INT_TIME_IN_MS		650
 #define ATLAS_ORP_INT_TIME_IN_MS	450
 #define ATLAS_DO_INT_TIME_IN_MS		450
+#define ATLAS_RTD_INT_TIME_IN_MS	450
 
 enum {
 	ATLAS_PH_SM,
 	ATLAS_EC_SM,
 	ATLAS_ORP_SM,
 	ATLAS_DO_SM,
+	ATLAS_RTD_SM,
 };
 
 struct atlas_data {
@@ -218,6 +222,22 @@ static const struct iio_chan_spec atlas_do_channels[] = {
 	},
 };
 
+static const struct iio_chan_spec atlas_rtd_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.address = ATLAS_REG_RTD_DATA,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED),
+		.scan_index = 0,
+		.scan_type = {
+			.sign = 's',
+			.realbits = 32,
+			.storagebits = 32,
+			.endianness = IIO_BE,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
 static int atlas_check_ph_calibration(struct atlas_data *data)
 {
 	struct device *dev = &data->client->dev;
@@ -362,6 +382,12 @@ static struct atlas_device atlas_devices[] = {
 				.calibration = &atlas_check_do_calibration,
 				.delay = ATLAS_DO_INT_TIME_IN_MS,
 	},
+	[ATLAS_RTD_SM] = {
+				.channels = atlas_rtd_channels,
+				.num_channels = 2,
+				.data_reg = ATLAS_REG_RTD_DATA,
+				.delay = ATLAS_RTD_INT_TIME_IN_MS,
+	},
 };
 
 static int atlas_set_powermode(struct atlas_data *data, int on)
@@ -438,8 +464,7 @@ static irqreturn_t atlas_trigger_handler(int irq, void *private)
 	int ret;
 
 	ret = regmap_bulk_read(data->regmap, data->chip->data_reg,
-			      (u8 *) &data->buffer,
-			      sizeof(__be32) * channels);
+			      &data->buffer, sizeof(__be32) * channels);
 
 	if (!ret)
 		iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
@@ -475,7 +500,7 @@ static int atlas_read_measurement(struct atlas_data *data, int reg, __be32 *val)
 	if (suspended)
 		msleep(data->chip->delay);
 
-	ret = regmap_bulk_read(data->regmap, reg, (u8 *) val, sizeof(*val));
+	ret = regmap_bulk_read(data->regmap, reg, val, sizeof(*val));
 
 	pm_runtime_mark_last_busy(dev);
 	pm_runtime_put_autosuspend(dev);
@@ -490,6 +515,7 @@ static int atlas_read_raw(struct iio_dev *indio_dev,
 	struct atlas_data *data = iio_priv(indio_dev);
 
 	switch (mask) {
+	case IIO_CHAN_INFO_PROCESSED:
 	case IIO_CHAN_INFO_RAW: {
 		int ret;
 		__be32 reg;
@@ -497,7 +523,7 @@ static int atlas_read_raw(struct iio_dev *indio_dev,
 		switch (chan->type) {
 		case IIO_TEMP:
 			ret = regmap_bulk_read(data->regmap, chan->address,
-					      (u8 *) &reg, sizeof(reg));
+					       &reg, sizeof(reg));
 			break;
 		case IIO_PH:
 		case IIO_CONCENTRATION:
@@ -578,6 +604,7 @@ static const struct i2c_device_id atlas_id[] = {
 	{ "atlas-ec-sm", ATLAS_EC_SM},
 	{ "atlas-orp-sm", ATLAS_ORP_SM},
 	{ "atlas-do-sm", ATLAS_DO_SM},
+	{ "atlas-rtd-sm", ATLAS_RTD_SM},
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, atlas_id);
@@ -587,6 +614,7 @@ static const struct of_device_id atlas_dt_ids[] = {
 	{ .compatible = "atlas,ec-sm", .data = (void *)ATLAS_EC_SM, },
 	{ .compatible = "atlas,orp-sm", .data = (void *)ATLAS_ORP_SM, },
 	{ .compatible = "atlas,do-sm", .data = (void *)ATLAS_DO_SM, },
+	{ .compatible = "atlas,rtd-sm", .data = (void *)ATLAS_RTD_SM, },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, atlas_dt_ids);
diff --git a/drivers/iio/chemical/bme680_core.c b/drivers/iio/chemical/bme680_core.c
index ccde4c65ff93..13773e01699b 100644
--- a/drivers/iio/chemical/bme680_core.c
+++ b/drivers/iio/chemical/bme680_core.c
@@ -114,14 +114,16 @@ static int bme680_read_calib(struct bme680_data *data,
 	__le16 buf;
 
 	/* Temperature related coefficients */
-	ret = regmap_bulk_read(data->regmap, BME680_T1_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_T1_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_T1_LSB_REG\n");
 		return ret;
 	}
 	calib->par_t1 = le16_to_cpu(buf);
 
-	ret = regmap_bulk_read(data->regmap, BME680_T2_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_T2_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_T2_LSB_REG\n");
 		return ret;
@@ -136,14 +138,16 @@ static int bme680_read_calib(struct bme680_data *data,
 	calib->par_t3 = tmp;
 
 	/* Pressure related coefficients */
-	ret = regmap_bulk_read(data->regmap, BME680_P1_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P1_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P1_LSB_REG\n");
 		return ret;
 	}
 	calib->par_p1 = le16_to_cpu(buf);
 
-	ret = regmap_bulk_read(data->regmap, BME680_P2_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P2_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P2_LSB_REG\n");
 		return ret;
@@ -157,14 +161,16 @@ static int bme680_read_calib(struct bme680_data *data,
 	}
 	calib->par_p3 = tmp;
 
-	ret = regmap_bulk_read(data->regmap, BME680_P4_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P4_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P4_LSB_REG\n");
 		return ret;
 	}
 	calib->par_p4 = le16_to_cpu(buf);
 
-	ret = regmap_bulk_read(data->regmap, BME680_P5_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P5_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P5_LSB_REG\n");
 		return ret;
@@ -185,14 +191,16 @@ static int bme680_read_calib(struct bme680_data *data,
 	}
 	calib->par_p7 = tmp;
 
-	ret = regmap_bulk_read(data->regmap, BME680_P8_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P8_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P8_LSB_REG\n");
 		return ret;
 	}
 	calib->par_p8 = le16_to_cpu(buf);
 
-	ret = regmap_bulk_read(data->regmap, BME680_P9_LSB_REG, (u8 *) &buf, 2);
+	ret = regmap_bulk_read(data->regmap, BME680_P9_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_P9_LSB_REG\n");
 		return ret;
@@ -276,8 +284,8 @@ static int bme680_read_calib(struct bme680_data *data,
 	}
 	calib->par_gh1 = tmp;
 
-	ret = regmap_bulk_read(data->regmap, BME680_GH2_LSB_REG, (u8 *) &buf,
-			       2);
+	ret = regmap_bulk_read(data->regmap, BME680_GH2_LSB_REG,
+			       &buf, sizeof(buf));
 	if (ret < 0) {
 		dev_err(dev, "failed to read BME680_GH2_LSB_REG\n");
 		return ret;
@@ -615,7 +623,7 @@ static int bme680_read_temp(struct bme680_data *data, int *val)
 		return ret;
 
 	ret = regmap_bulk_read(data->regmap, BME680_REG_TEMP_MSB,
-			       (u8 *) &tmp, 3);
+			       &tmp, 3);
 	if (ret < 0) {
 		dev_err(dev, "failed to read temperature\n");
 		return ret;
@@ -656,7 +664,7 @@ static int bme680_read_press(struct bme680_data *data,
 		return ret;
 
 	ret = regmap_bulk_read(data->regmap, BME680_REG_PRESS_MSB,
-			       (u8 *) &tmp, 3);
+			       &tmp, 3);
 	if (ret < 0) {
 		dev_err(dev, "failed to read pressure\n");
 		return ret;
@@ -689,7 +697,7 @@ static int bme680_read_humid(struct bme680_data *data,
 		return ret;
 
 	ret = regmap_bulk_read(data->regmap, BM6880_REG_HUMIDITY_MSB,
-			       (u8 *) &tmp, 2);
+			       &tmp, sizeof(tmp));
 	if (ret < 0) {
 		dev_err(dev, "failed to read humidity\n");
 		return ret;
@@ -754,7 +762,7 @@ static int bme680_read_gas(struct bme680_data *data,
 	}
 
 	ret = regmap_bulk_read(data->regmap, BME680_REG_GAS_MSB,
-			       (u8 *) &tmp, 2);
+			       &tmp, sizeof(tmp));
 	if (ret < 0) {
 		dev_err(dev, "failed to read gas resistance\n");
 		return ret;
diff --git a/drivers/iio/chemical/ccs811.c b/drivers/iio/chemical/ccs811.c
index 2ebdfc35bcda..3ecd633f9ed3 100644
--- a/drivers/iio/chemical/ccs811.c
+++ b/drivers/iio/chemical/ccs811.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/delay.h>
+#include <linux/gpio/consumer.h>
 #include <linux/i2c.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
@@ -36,6 +37,7 @@
 #define CCS811_ERR		0xE0
 /* Used to transition from boot to application mode */
 #define CCS811_APP_START	0xF4
+#define CCS811_SW_RESET		0xFF
 
 /* Status register flags */
 #define CCS811_STATUS_ERROR		BIT(0)
@@ -74,6 +76,7 @@ struct ccs811_data {
 	struct mutex lock; /* Protect readings */
 	struct ccs811_reading buffer;
 	struct iio_trigger *drdy_trig;
+	struct gpio_desc *wakeup_gpio;
 	bool drdy_trig_on;
 };
 
@@ -166,10 +169,25 @@ static int ccs811_setup(struct i2c_client *client)
 					 CCS811_MODE_IAQ_1SEC);
 }
 
+static void ccs811_set_wakeup(struct ccs811_data *data, bool enable)
+{
+	if (!data->wakeup_gpio)
+		return;
+
+	gpiod_set_value(data->wakeup_gpio, enable);
+
+	if (enable)
+		usleep_range(50, 60);
+	else
+		usleep_range(20, 30);
+}
+
 static int ccs811_get_measurement(struct ccs811_data *data)
 {
 	int ret, tries = 11;
 
+	ccs811_set_wakeup(data, true);
+
 	/* Maximum waiting time: 1s, as measurements are made every second */
 	while (tries-- > 0) {
 		ret = i2c_smbus_read_byte_data(data->client, CCS811_STATUS);
@@ -183,9 +201,12 @@ static int ccs811_get_measurement(struct ccs811_data *data)
 	if (!(ret & CCS811_STATUS_DATA_READY))
 		return -EIO;
 
-	return i2c_smbus_read_i2c_block_data(data->client,
+	ret = i2c_smbus_read_i2c_block_data(data->client,
 					    CCS811_ALG_RESULT_DATA, 8,
 					    (char *)&data->buffer);
+	ccs811_set_wakeup(data, false);
+
+	return ret;
 }
 
 static int ccs811_read_raw(struct iio_dev *indio_dev,
@@ -336,6 +357,45 @@ static irqreturn_t ccs811_data_rdy_trigger_poll(int irq, void *private)
 	return IRQ_HANDLED;
 }
 
+static int ccs811_reset(struct i2c_client *client)
+{
+	struct gpio_desc *reset_gpio;
+	int ret;
+
+	reset_gpio = devm_gpiod_get_optional(&client->dev, "reset",
+					     GPIOD_OUT_LOW);
+	if (IS_ERR(reset_gpio))
+		return PTR_ERR(reset_gpio);
+
+	/* Try to reset using nRESET pin if available else do SW reset */
+	if (reset_gpio) {
+		gpiod_set_value(reset_gpio, 1);
+		usleep_range(20, 30);
+		gpiod_set_value(reset_gpio, 0);
+	} else {
+		/*
+		 * As per the datasheet, this sequence of values needs to be
+		 * written to the SW_RESET register for triggering the soft
+		 * reset in the device and placing it in boot mode.
+		 */
+		static const u8 reset_seq[] = {
+			0x11, 0xE5, 0x72, 0x8A,
+		};
+
+		ret = i2c_smbus_write_i2c_block_data(client, CCS811_SW_RESET,
+					     sizeof(reset_seq), reset_seq);
+		if (ret < 0) {
+			dev_err(&client->dev, "Failed to reset sensor\n");
+			return ret;
+		}
+	}
+
+	/* tSTART delay required after reset */
+	usleep_range(1000, 2000);
+
+	return 0;
+}
+
 static int ccs811_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -348,36 +408,59 @@ static int ccs811_probe(struct i2c_client *client,
 				     | I2C_FUNC_SMBUS_READ_I2C_BLOCK))
 		return -EOPNOTSUPP;
 
+	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;
+
+	data->wakeup_gpio = devm_gpiod_get_optional(&client->dev, "wakeup",
+						    GPIOD_OUT_HIGH);
+	if (IS_ERR(data->wakeup_gpio))
+		return PTR_ERR(data->wakeup_gpio);
+
+	ccs811_set_wakeup(data, true);
+
+	ret = ccs811_reset(client);
+	if (ret) {
+		ccs811_set_wakeup(data, false);
+		return ret;
+	}
+
 	/* Check hardware id (should be 0x81 for this family of devices) */
 	ret = i2c_smbus_read_byte_data(client, CCS811_HW_ID);
-	if (ret < 0)
+	if (ret < 0) {
+		ccs811_set_wakeup(data, false);
 		return ret;
+	}
 
 	if (ret != CCS811_HW_ID_VALUE) {
 		dev_err(&client->dev, "hardware id doesn't match CCS81x\n");
+		ccs811_set_wakeup(data, false);
 		return -ENODEV;
 	}
 
 	ret = i2c_smbus_read_byte_data(client, CCS811_HW_VERSION);
-	if (ret < 0)
+	if (ret < 0) {
+		ccs811_set_wakeup(data, false);
 		return ret;
+	}
 
 	if ((ret & CCS811_HW_VERSION_MASK) != CCS811_HW_VERSION_VALUE) {
 		dev_err(&client->dev, "no CCS811 sensor\n");
+		ccs811_set_wakeup(data, false);
 		return -ENODEV;
 	}
 
-	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
-	if (!indio_dev)
-		return -ENOMEM;
-
 	ret = ccs811_setup(client);
-	if (ret < 0)
+	if (ret < 0) {
+		ccs811_set_wakeup(data, false);
 		return ret;
+	}
 
-	data = iio_priv(indio_dev);
-	i2c_set_clientdata(client, indio_dev);
-	data->client = client;
+	ccs811_set_wakeup(data, false);
 
 	mutex_init(&data->lock);
 
@@ -466,9 +549,16 @@ static const struct i2c_device_id ccs811_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, ccs811_id);
 
+static const struct of_device_id ccs811_dt_ids[] = {
+	{ .compatible = "ams,ccs811" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ccs811_dt_ids);
+
 static struct i2c_driver ccs811_driver = {
 	.driver = {
 		.name = "ccs811",
+		.of_match_table = ccs811_dt_ids,
 	},
 	.probe = ccs811_probe,
 	.remove = ccs811_remove,
diff --git a/drivers/iio/chemical/pms7003.c b/drivers/iio/chemical/pms7003.c
index 23c9ab252470..07bb90d72434 100644
--- a/drivers/iio/chemical/pms7003.c
+++ b/drivers/iio/chemical/pms7003.c
@@ -73,6 +73,11 @@ struct pms7003_state {
 	struct pms7003_frame frame;
 	struct completion frame_ready;
 	struct mutex lock; /* must be held whenever state gets touched */
+	/* Used to construct scan to push to the IIO buffer */
+	struct {
+		u16 data[3]; /* PM1, PM2P5, PM10 */
+		s64 ts;
+	} scan;
 };
 
 static int pms7003_do_cmd(struct pms7003_state *state, enum pms7003_cmd cmd)
@@ -104,7 +109,6 @@ static irqreturn_t pms7003_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct pms7003_state *state = iio_priv(indio_dev);
 	struct pms7003_frame *frame = &state->frame;
-	u16 data[3 + 1 + 4]; /* PM1, PM2P5, PM10, padding, timestamp */
 	int ret;
 
 	mutex_lock(&state->lock);
@@ -114,12 +118,15 @@ static irqreturn_t pms7003_trigger_handler(int irq, void *p)
 		goto err;
 	}
 
-	data[PM1] = pms7003_get_pm(frame->data + PMS7003_PM1_OFFSET);
-	data[PM2P5] = pms7003_get_pm(frame->data + PMS7003_PM2P5_OFFSET);
-	data[PM10] = pms7003_get_pm(frame->data + PMS7003_PM10_OFFSET);
+	state->scan.data[PM1] =
+		pms7003_get_pm(frame->data + PMS7003_PM1_OFFSET);
+	state->scan.data[PM2P5] =
+		pms7003_get_pm(frame->data + PMS7003_PM2P5_OFFSET);
+	state->scan.data[PM10] =
+		pms7003_get_pm(frame->data + PMS7003_PM10_OFFSET);
 	mutex_unlock(&state->lock);
 
-	iio_push_to_buffers_with_timestamp(indio_dev, data,
+	iio_push_to_buffers_with_timestamp(indio_dev, &state->scan,
 					   iio_get_time_ns(indio_dev));
 err:
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/chemical/sps30.c b/drivers/iio/chemical/sps30.c
index acb9f8ecbb3d..a88c1fb875a0 100644
--- a/drivers/iio/chemical/sps30.c
+++ b/drivers/iio/chemical/sps30.c
@@ -230,15 +230,18 @@ static irqreturn_t sps30_trigger_handler(int irq, void *p)
 	struct iio_dev *indio_dev = pf->indio_dev;
 	struct sps30_state *state = iio_priv(indio_dev);
 	int ret;
-	s32 data[4 + 2]; /* PM1, PM2P5, PM4, PM10, timestamp */
+	struct {
+		s32 data[4]; /* PM1, PM2P5, PM4, PM10 */
+		s64 ts;
+	} scan;
 
 	mutex_lock(&state->lock);
-	ret = sps30_do_meas(state, data, 4);
+	ret = sps30_do_meas(state, scan.data, ARRAY_SIZE(scan.data));
 	mutex_unlock(&state->lock);
 	if (ret)
 		goto err;
 
-	iio_push_to_buffers_with_timestamp(indio_dev, data,
+	iio_push_to_buffers_with_timestamp(indio_dev, &scan,
 					   iio_get_time_ns(indio_dev));
 err:
 	iio_trigger_notify_done(indio_dev->trig);
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index 906d87780419..ff375790b7e8 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -13,6 +13,8 @@
 #include <linux/hid-sensor-hub.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
 #include <linux/iio/buffer.h>
 #include <linux/iio/sysfs.h>
 #include "hid-sensor-trigger.h"
@@ -222,7 +224,8 @@ static int hid_sensor_data_rdy_trigger_set_state(struct iio_trigger *trig,
 	return hid_sensor_power_state(iio_trigger_get_drvdata(trig), state);
 }
 
-void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
+void hid_sensor_remove_trigger(struct iio_dev *indio_dev,
+			       struct hid_sensor_common *attrb)
 {
 	if (atomic_read(&attrb->runtime_pm_enable))
 		pm_runtime_disable(&attrb->pdev->dev);
@@ -233,6 +236,7 @@ void hid_sensor_remove_trigger(struct hid_sensor_common *attrb)
 	cancel_work_sync(&attrb->work);
 	iio_trigger_unregister(attrb->trigger);
 	iio_trigger_free(attrb->trigger);
+	iio_triggered_buffer_cleanup(indio_dev);
 }
 EXPORT_SYMBOL(hid_sensor_remove_trigger);
 
@@ -246,11 +250,18 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 	int ret;
 	struct iio_trigger *trig;
 
+	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
+					 NULL, NULL);
+	if (ret) {
+		dev_err(&indio_dev->dev, "Triggered Buffer Setup Failed\n");
+		return ret;
+	}
+
 	trig = iio_trigger_alloc("%s-dev%d", name, indio_dev->id);
 	if (trig == NULL) {
 		dev_err(&indio_dev->dev, "Trigger Allocate Failed\n");
 		ret = -ENOMEM;
-		goto error_ret;
+		goto error_triggered_buffer_cleanup;
 	}
 
 	trig->dev.parent = indio_dev->dev.parent;
@@ -284,7 +295,8 @@ error_unreg_trigger:
 	iio_trigger_unregister(trig);
 error_free_trig:
 	iio_trigger_free(trig);
-error_ret:
+error_triggered_buffer_cleanup:
+	iio_triggered_buffer_cleanup(indio_dev);
 	return ret;
 }
 EXPORT_SYMBOL(hid_sensor_setup_trigger);
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
index f47b940ff170..bb45cc89e551 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.h
@@ -13,7 +13,8 @@ extern const struct dev_pm_ops hid_sensor_pm_ops;
 
 int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 				struct hid_sensor_common *attrb);
-void hid_sensor_remove_trigger(struct hid_sensor_common *attrb);
+void hid_sensor_remove_trigger(struct iio_dev *indio_dev,
+			       struct hid_sensor_common *attrb);
 int hid_sensor_power_state(struct hid_sensor_common *st, bool state);
 
 #endif
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 13bdfbbf5f71..7a69c1be7393 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -20,11 +20,6 @@
 
 #include "st_sensors_core.h"
 
-static inline u32 st_sensors_get_unaligned_le24(const u8 *p)
-{
-	return (s32)((p[0] | p[1] << 8 | p[2] << 16) << 8) >> 8;
-}
-
 int st_sensors_write_data_with_mask(struct iio_dev *indio_dev,
 				    u8 reg_addr, u8 mask, u8 data)
 {
@@ -150,8 +145,7 @@ static int st_sensors_set_fullscale(struct iio_dev *indio_dev, unsigned int fs)
 	if (err < 0)
 		goto st_accel_set_fullscale_error;
 
-	sdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-					&sdata->sensor_settings->fs.fs_avl[i];
+	sdata->current_fullscale = &sdata->sensor_settings->fs.fs_avl[i];
 	return err;
 
 st_accel_set_fullscale_error:
@@ -278,8 +272,7 @@ static int st_sensors_set_drdy_int_pin(struct iio_dev *indio_dev,
 	    !sdata->sensor_settings->drdy_irq.int2.addr) {
 		if (pdata->drdy_int_pin)
 			dev_info(&indio_dev->dev,
-				 "DRDY on pin INT%d specified, but sensor "
-				 "does not support interrupts\n",
+				 "DRDY on pin INT%d specified, but sensor does not support interrupts\n",
 				 pdata->drdy_int_pin);
 		return 0;
 	}
@@ -545,7 +538,7 @@ static int st_sensors_read_axis_data(struct iio_dev *indio_dev,
 	else if (byte_for_channel == 2)
 		*data = (s16)get_unaligned_le16(outdata);
 	else if (byte_for_channel == 3)
-		*data = (s32)st_sensors_get_unaligned_le24(outdata);
+		*data = (s32)sign_extend32(get_unaligned_le24(outdata), 23);
 
 st_sensors_free_memory:
 	kfree(outdata);
diff --git a/drivers/iio/common/st_sensors/st_sensors_i2c.c b/drivers/iio/common/st_sensors/st_sensors_i2c.c
index 286830fb5d35..b400560bac93 100644
--- a/drivers/iio/common/st_sensors/st_sensors_i2c.c
+++ b/drivers/iio/common/st_sensors/st_sensors_i2c.c
@@ -49,8 +49,8 @@ int st_sensors_i2c_configure(struct iio_dev *indio_dev,
 
 	sdata->regmap = devm_regmap_init_i2c(client, config);
 	if (IS_ERR(sdata->regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap (%d)\n",
-			(int)PTR_ERR(sdata->regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap (%ld)\n",
+			PTR_ERR(sdata->regmap));
 		return PTR_ERR(sdata->regmap);
 	}
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_spi.c b/drivers/iio/common/st_sensors/st_sensors_spi.c
index 1275fb0eda31..ee70515bb89f 100644
--- a/drivers/iio/common/st_sensors/st_sensors_spi.c
+++ b/drivers/iio/common/st_sensors/st_sensors_spi.c
@@ -44,7 +44,7 @@ static bool st_sensors_is_spi_3_wire(struct spi_device *spi)
 	if (device_property_read_bool(dev, "spi-3wire"))
 		return true;
 
-	pdata = (struct st_sensors_platform_data *)dev->platform_data;
+	pdata = dev_get_platdata(dev);
 	if (pdata && pdata->spi_3wire)
 		return true;
 
@@ -101,8 +101,8 @@ int st_sensors_spi_configure(struct iio_dev *indio_dev,
 
 	sdata->regmap = devm_regmap_init_spi(spi, config);
 	if (IS_ERR(sdata->regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap (%d)\n",
-			(int)PTR_ERR(sdata->regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap (%ld)\n",
+			PTR_ERR(sdata->regmap));
 		return PTR_ERR(sdata->regmap);
 	}
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index e817537cdfb5..0507283bd4c1 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -44,8 +44,7 @@ static int st_sensors_new_samples_available(struct iio_dev *indio_dev,
 			  sdata->sensor_settings->drdy_irq.stat_drdy.addr,
 			  &status);
 	if (ret < 0) {
-		dev_err(sdata->dev,
-			"error checking samples available\n");
+		dev_err(sdata->dev, "error checking samples available\n");
 		return ret;
 	}
 
@@ -148,9 +147,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 	case IRQF_TRIGGER_LOW:
 		if (!sdata->sensor_settings->drdy_irq.addr_ihl) {
 			dev_err(&indio_dev->dev,
-				"falling/low specified for IRQ "
-				"but hardware supports only rising/high: "
-				"will request rising/high\n");
+				"falling/low specified for IRQ but hardware supports only rising/high: will request rising/high\n");
 			if (irq_trig == IRQF_TRIGGER_FALLING)
 				irq_trig = IRQF_TRIGGER_RISING;
 			if (irq_trig == IRQF_TRIGGER_LOW)
@@ -163,8 +160,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 			if (err < 0)
 				goto iio_trigger_free;
 			dev_info(&indio_dev->dev,
-				 "interrupts on the falling edge or "
-				 "active low level\n");
+				 "interrupts on the falling edge or active low level\n");
 		}
 		break;
 	case IRQF_TRIGGER_RISING:
@@ -178,8 +174,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 	default:
 		/* This is the most preferred mode, if possible */
 		dev_err(&indio_dev->dev,
-			"unsupported IRQ trigger specified (%lx), enforce "
-			"rising edge\n", irq_trig);
+			"unsupported IRQ trigger specified (%lx), enforce rising edge\n", irq_trig);
 		irq_trig = IRQF_TRIGGER_RISING;
 	}
 
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index 93744011b63f..3728f6325501 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -279,12 +279,12 @@ config LTC1660
 	  module will be called ltc1660.
 
 config LTC2632
-	tristate "Linear Technology LTC2632-12/10/8 and LTC2636-12/10/8 DAC spi driver"
+	tristate "Linear Technology LTC2632-12/10/8 and similar DAC spi driver"
 	depends on SPI
 	help
 	  Say yes here to build support for Linear Technology
-	  LTC2632-12, LTC2632-10, LTC2632-8, LTC2636-12, LTC2636-10 and
-	  LTC2636-8 converters (DAC).
+	  LTC2632, LTC2634 and LTC2636 DAC resolution 12/10/8 bit
+	  low 0-2.5V and high 0-4.096V range converters.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called ltc2632.
diff --git a/drivers/iio/dac/ad5360.c b/drivers/iio/dac/ad5360.c
index 2ac428b957e3..3e0c9e84e8da 100644
--- a/drivers/iio/dac/ad5360.c
+++ b/drivers/iio/dac/ad5360.c
@@ -67,6 +67,7 @@ struct ad5360_chip_info {
  * @chip_info:		chip model specific constants, available modes etc
  * @vref_reg:		vref supply regulators
  * @ctrl:		control register cache
+ * @lock		lock to protect the data buffer during SPI ops
  * @data:		spi transfer buffers
  */
 
@@ -75,6 +76,7 @@ struct ad5360_state {
 	const struct ad5360_chip_info	*chip_info;
 	struct regulator_bulk_data	vref_reg[3];
 	unsigned int			ctrl;
+	struct mutex			lock;
 
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
@@ -205,10 +207,11 @@ static int ad5360_write(struct iio_dev *indio_dev, unsigned int cmd,
 	unsigned int addr, unsigned int val, unsigned int shift)
 {
 	int ret;
+	struct ad5360_state *st = iio_priv(indio_dev);
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad5360_write_unlocked(indio_dev, cmd, addr, val, shift);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -229,7 +232,7 @@ static int ad5360_read(struct iio_dev *indio_dev, unsigned int type,
 		},
 	};
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->data[0].d32 = cpu_to_be32(AD5360_CMD(AD5360_CMD_SPECIAL_FUNCTION) |
 		AD5360_ADDR(AD5360_REG_SF_READBACK) |
@@ -240,7 +243,7 @@ static int ad5360_read(struct iio_dev *indio_dev, unsigned int type,
 	if (ret >= 0)
 		ret = be32_to_cpu(st->data[1].d32) & 0xffff;
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -261,7 +264,7 @@ static int ad5360_update_ctrl(struct iio_dev *indio_dev, unsigned int set,
 	struct ad5360_state *st = iio_priv(indio_dev);
 	unsigned int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->ctrl |= set;
 	st->ctrl &= ~clr;
@@ -269,7 +272,7 @@ static int ad5360_update_ctrl(struct iio_dev *indio_dev, unsigned int set,
 	ret = ad5360_write_unlocked(indio_dev, AD5360_CMD_SPECIAL_FUNCTION,
 			AD5360_REG_SF_CTRL, st->ctrl, 0);
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -479,6 +482,8 @@ static int ad5360_probe(struct spi_device *spi)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
+	mutex_init(&st->lock);
+
 	ret = ad5360_alloc_channels(indio_dev);
 	if (ret) {
 		dev_err(&spi->dev, "Failed to allocate channel spec: %d\n", ret);
diff --git a/drivers/iio/dac/ad5380.c b/drivers/iio/dac/ad5380.c
index 2ebe08326048..b37e5675f716 100644
--- a/drivers/iio/dac/ad5380.c
+++ b/drivers/iio/dac/ad5380.c
@@ -51,6 +51,7 @@ struct ad5380_chip_info {
  * @vref_reg:		vref supply regulator
  * @vref:		actual reference voltage used in uA
  * @pwr_down:		whether the chip is currently in power down mode
+ * @lock		lock to protect the data buffer during regmap ops
  */
 
 struct ad5380_state {
@@ -59,6 +60,7 @@ struct ad5380_state {
 	struct regulator		*vref_reg;
 	int				vref;
 	bool				pwr_down;
+	struct mutex			lock;
 };
 
 enum ad5380_type {
@@ -98,7 +100,7 @@ static ssize_t ad5380_write_dac_powerdown(struct iio_dev *indio_dev,
 	if (ret)
 		return ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	if (pwr_down)
 		ret = regmap_write(st->regmap, AD5380_REG_SF_PWR_DOWN, 0);
@@ -107,7 +109,7 @@ static ssize_t ad5380_write_dac_powerdown(struct iio_dev *indio_dev,
 
 	st->pwr_down = pwr_down;
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -390,6 +392,8 @@ static int ad5380_probe(struct device *dev, struct regmap *regmap,
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
+	mutex_init(&st->lock);
+
 	ret = ad5380_alloc_channels(indio_dev);
 	if (ret) {
 		dev_err(dev, "Failed to allocate channel spec: %d\n", ret);
diff --git a/drivers/iio/dac/ad5421.c b/drivers/iio/dac/ad5421.c
index 63063e85cd0a..fec27764cea8 100644
--- a/drivers/iio/dac/ad5421.c
+++ b/drivers/iio/dac/ad5421.c
@@ -62,12 +62,14 @@
  * @current_range:	current range which the device is configured for
  * @data:		spi transfer buffers
  * @fault_mask:		software masking of events
+ * @lock	lock to protect the data buffer during SPI ops
  */
 struct ad5421_state {
 	struct spi_device		*spi;
 	unsigned int			ctrl;
 	enum ad5421_current_range	current_range;
 	unsigned int			fault_mask;
+	struct mutex			lock;
 
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
@@ -142,11 +144,12 @@ static int ad5421_write_unlocked(struct iio_dev *indio_dev,
 static int ad5421_write(struct iio_dev *indio_dev, unsigned int reg,
 	unsigned int val)
 {
+	struct ad5421_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad5421_write_unlocked(indio_dev, reg, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -166,7 +169,7 @@ static int ad5421_read(struct iio_dev *indio_dev, unsigned int reg)
 		},
 	};
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->data[0].d32 = cpu_to_be32((1 << 23) | (reg << 16));
 
@@ -174,7 +177,7 @@ static int ad5421_read(struct iio_dev *indio_dev, unsigned int reg)
 	if (ret >= 0)
 		ret = be32_to_cpu(st->data[1].d32) & 0xffff;
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -185,14 +188,14 @@ static int ad5421_update_ctrl(struct iio_dev *indio_dev, unsigned int set,
 	struct ad5421_state *st = iio_priv(indio_dev);
 	unsigned int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->ctrl &= ~clr;
 	st->ctrl |= set;
 
 	ret = ad5421_write_unlocked(indio_dev, AD5421_REG_CTRL, st->ctrl);
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -400,12 +403,12 @@ static int ad5421_write_event_config(struct iio_dev *indio_dev,
 		return -EINVAL;
 	}
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	if (state)
 		st->fault_mask |= mask;
 	else
 		st->fault_mask &= ~mask;
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return 0;
 }
@@ -491,6 +494,8 @@ static int ad5421_probe(struct spi_device *spi)
 	indio_dev->channels = ad5421_channels;
 	indio_dev->num_channels = ARRAY_SIZE(ad5421_channels);
 
+	mutex_init(&st->lock);
+
 	st->ctrl = AD5421_CTRL_WATCHDOG_DISABLE |
 			AD5421_CTRL_AUTO_FAULT_READBACK;
 
diff --git a/drivers/iio/dac/ad5446.c b/drivers/iio/dac/ad5446.c
index 61c670f7fc5f..8f8afc8999bc 100644
--- a/drivers/iio/dac/ad5446.c
+++ b/drivers/iio/dac/ad5446.c
@@ -21,6 +21,8 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+#include <asm/unaligned.h>
+
 #define MODE_PWRDWN_1k		0x1
 #define MODE_PWRDWN_100k	0x2
 #define MODE_PWRDWN_TRISTATE	0x3
@@ -31,6 +33,7 @@
  * @chip_info:		chip model specific constants, available modes etc
  * @reg:		supply regulator
  * @vref_mv:		actual reference voltage used
+ * @lock		lock to protect the data buffer during write ops
  */
 
 struct ad5446_state {
@@ -41,6 +44,7 @@ struct ad5446_state {
 	unsigned			cached_val;
 	unsigned			pwr_down_mode;
 	unsigned			pwr_down;
+	struct mutex			lock;
 };
 
 /**
@@ -110,7 +114,7 @@ static ssize_t ad5446_write_dac_powerdown(struct iio_dev *indio_dev,
 	if (ret)
 		return ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	st->pwr_down = powerdown;
 
 	if (st->pwr_down) {
@@ -121,7 +125,7 @@ static ssize_t ad5446_write_dac_powerdown(struct iio_dev *indio_dev,
 	}
 
 	ret = st->chip_info->write(st, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret ? ret : len;
 }
@@ -195,11 +199,11 @@ static int ad5446_write_raw(struct iio_dev *indio_dev,
 			return -EINVAL;
 
 		val <<= chan->scan_type.shift;
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		st->cached_val = val;
 		if (!st->pwr_down)
 			ret = st->chip_info->write(st, val);
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 		break;
 	default:
 		ret = -EINVAL;
@@ -254,6 +258,8 @@ static int ad5446_probe(struct device *dev, const char *name,
 	indio_dev->channels = &st->chip_info->channel;
 	indio_dev->num_channels = 1;
 
+	mutex_init(&st->lock);
+
 	st->pwr_down_mode = MODE_PWRDWN_1k;
 
 	if (st->chip_info->int_vref_mv)
@@ -302,9 +308,7 @@ static int ad5660_write(struct ad5446_state *st, unsigned val)
 	struct spi_device *spi = to_spi_device(st->dev);
 	uint8_t data[3];
 
-	data[0] = (val >> 16) & 0xFF;
-	data[1] = (val >> 8) & 0xFF;
-	data[2] = val & 0xFF;
+	put_unaligned_be24(val, &data[0]);
 
 	return spi_write(spi, data, sizeof(data));
 }
diff --git a/drivers/iio/dac/ad5449.c b/drivers/iio/dac/ad5449.c
index fed3ebaccac4..d739b10e5236 100644
--- a/drivers/iio/dac/ad5449.c
+++ b/drivers/iio/dac/ad5449.c
@@ -56,11 +56,13 @@ struct ad5449_chip_info {
  * @has_sdo:		whether the SDO line is connected
  * @dac_cache:		Cache for the DAC values
  * @data:		spi transfer buffers
+ * @lock		lock to protect the data buffer during SPI ops
  */
 struct ad5449 {
 	struct spi_device		*spi;
 	const struct ad5449_chip_info	*chip_info;
 	struct regulator_bulk_data	vref_reg[AD5449_MAX_VREFS];
+	struct mutex			lock;
 
 	bool has_sdo;
 	uint16_t dac_cache[AD5449_MAX_CHANNELS];
@@ -87,10 +89,10 @@ static int ad5449_write(struct iio_dev *indio_dev, unsigned int addr,
 	struct ad5449 *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	st->data[0] = cpu_to_be16((addr << 12) | val);
 	ret = spi_write(st->spi, st->data, 2);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -112,7 +114,7 @@ static int ad5449_read(struct iio_dev *indio_dev, unsigned int addr,
 		},
 	};
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	st->data[0] = cpu_to_be16(addr << 12);
 	st->data[1] = cpu_to_be16(AD5449_CMD_NOOP);
 
@@ -123,7 +125,7 @@ static int ad5449_read(struct iio_dev *indio_dev, unsigned int addr,
 	*val = be16_to_cpu(st->data[1]);
 
 out_unlock:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 	return ret;
 }
 
@@ -302,6 +304,8 @@ static int ad5449_spi_probe(struct spi_device *spi)
 	indio_dev->channels = st->chip_info->channels;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
+	mutex_init(&st->lock);
+
 	if (st->chip_info->has_ctrl) {
 		unsigned int ctrl = 0x00;
 		if (pdata) {
diff --git a/drivers/iio/dac/ad5592r-base.c b/drivers/iio/dac/ad5592r-base.c
index e2110113e884..410e90e5f75f 100644
--- a/drivers/iio/dac/ad5592r-base.c
+++ b/drivers/iio/dac/ad5592r-base.c
@@ -156,7 +156,6 @@ static void ad5592r_gpio_cleanup(struct ad5592r_state *st)
 static int ad5592r_reset(struct ad5592r_state *st)
 {
 	struct gpio_desc *gpio;
-	struct iio_dev *iio_dev = iio_priv_to_dev(st);
 
 	gpio = devm_gpiod_get_optional(st->dev, "reset", GPIOD_OUT_LOW);
 	if (IS_ERR(gpio))
@@ -166,10 +165,10 @@ static int ad5592r_reset(struct ad5592r_state *st)
 		udelay(1);
 		gpiod_set_value(gpio, 1);
 	} else {
-		mutex_lock(&iio_dev->mlock);
+		mutex_lock(&st->lock);
 		/* Writing this magic value resets the device */
 		st->ops->reg_write(st, AD5592R_REG_RESET, 0xdac);
-		mutex_unlock(&iio_dev->mlock);
+		mutex_unlock(&st->lock);
 	}
 
 	udelay(250);
@@ -197,7 +196,6 @@ static int ad5592r_set_channel_modes(struct ad5592r_state *st)
 	const struct ad5592r_rw_ops *ops = st->ops;
 	int ret;
 	unsigned i;
-	struct iio_dev *iio_dev = iio_priv_to_dev(st);
 	u8 pulldown = 0, tristate = 0, dac = 0, adc = 0;
 	u16 read_back;
 
@@ -247,7 +245,7 @@ static int ad5592r_set_channel_modes(struct ad5592r_state *st)
 		}
 	}
 
-	mutex_lock(&iio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	/* Pull down unused pins to GND */
 	ret = ops->reg_write(st, AD5592R_REG_PULLDOWN, pulldown);
@@ -285,7 +283,7 @@ static int ad5592r_set_channel_modes(struct ad5592r_state *st)
 		ret = -EIO;
 
 err_unlock:
-	mutex_unlock(&iio_dev->mlock);
+	mutex_unlock(&st->lock);
 	return ret;
 }
 
@@ -314,11 +312,11 @@ static int ad5592r_write_raw(struct iio_dev *iio_dev,
 		if (!chan->output)
 			return -EINVAL;
 
-		mutex_lock(&iio_dev->mlock);
+		mutex_lock(&st->lock);
 		ret = st->ops->write_dac(st, chan->channel, val);
 		if (!ret)
 			st->cached_dac[chan->channel] = val;
-		mutex_unlock(&iio_dev->mlock);
+		mutex_unlock(&st->lock);
 		return ret;
 	case IIO_CHAN_INFO_SCALE:
 		if (chan->type == IIO_VOLTAGE) {
@@ -333,12 +331,12 @@ static int ad5592r_write_raw(struct iio_dev *iio_dev,
 			else
 				return -EINVAL;
 
-			mutex_lock(&iio_dev->mlock);
+			mutex_lock(&st->lock);
 
 			ret = st->ops->reg_read(st, AD5592R_REG_CTRL,
 						&st->cached_gp_ctrl);
 			if (ret < 0) {
-				mutex_unlock(&iio_dev->mlock);
+				mutex_unlock(&st->lock);
 				return ret;
 			}
 
@@ -360,7 +358,7 @@ static int ad5592r_write_raw(struct iio_dev *iio_dev,
 
 			ret = st->ops->reg_write(st, AD5592R_REG_CTRL,
 						 st->cached_gp_ctrl);
-			mutex_unlock(&iio_dev->mlock);
+			mutex_unlock(&st->lock);
 
 			return ret;
 		}
@@ -382,7 +380,7 @@ static int ad5592r_read_raw(struct iio_dev *iio_dev,
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&iio_dev->mlock);
+		mutex_lock(&st->lock);
 
 		if (!chan->output) {
 			ret = st->ops->read_adc(st, chan->channel, &read_val);
@@ -419,7 +417,7 @@ static int ad5592r_read_raw(struct iio_dev *iio_dev,
 		} else {
 			int mult;
 
-			mutex_lock(&iio_dev->mlock);
+			mutex_lock(&st->lock);
 
 			if (chan->output)
 				mult = !!(st->cached_gp_ctrl &
@@ -437,7 +435,7 @@ static int ad5592r_read_raw(struct iio_dev *iio_dev,
 	case IIO_CHAN_INFO_OFFSET:
 		ret = ad5592r_get_vref(st);
 
-		mutex_lock(&iio_dev->mlock);
+		mutex_lock(&st->lock);
 
 		if (st->cached_gp_ctrl & AD5592R_REG_CTRL_ADC_RANGE)
 			*val = (-34365 * 25) / ret;
@@ -450,7 +448,7 @@ static int ad5592r_read_raw(struct iio_dev *iio_dev,
 	}
 
 unlock:
-	mutex_unlock(&iio_dev->mlock);
+	mutex_unlock(&st->lock);
 	return ret;
 }
 
@@ -625,6 +623,8 @@ int ad5592r_probe(struct device *dev, const char *name,
 	iio_dev->info = &ad5592r_info;
 	iio_dev->modes = INDIO_DIRECT_MODE;
 
+	mutex_init(&st->lock);
+
 	ad5592r_init_scales(st, ad5592r_get_vref(st));
 
 	ret = ad5592r_reset(st);
diff --git a/drivers/iio/dac/ad5592r-base.h b/drivers/iio/dac/ad5592r-base.h
index 4774e4cd9c11..23dac2f1ff8a 100644
--- a/drivers/iio/dac/ad5592r-base.h
+++ b/drivers/iio/dac/ad5592r-base.h
@@ -52,6 +52,7 @@ struct ad5592r_state {
 	struct regulator *reg;
 	struct gpio_chip gpiochip;
 	struct mutex gpio_lock;	/* Protect cached gpio_out, gpio_val, etc. */
+	struct mutex lock;
 	unsigned int num_channels;
 	const struct ad5592r_rw_ops *ops;
 	int scale_avail[2][2];
diff --git a/drivers/iio/dac/ad5592r.c b/drivers/iio/dac/ad5592r.c
index 34ba059a77da..49308ad13c4b 100644
--- a/drivers/iio/dac/ad5592r.c
+++ b/drivers/iio/dac/ad5592r.c
@@ -98,7 +98,7 @@ static int ad5592r_reg_read(struct ad5592r_state *st, u8 reg, u16 *value)
 	return 0;
 }
 
-static int ad5593r_gpio_read(struct ad5592r_state *st, u8 *value)
+static int ad5592r_gpio_read(struct ad5592r_state *st, u8 *value)
 {
 	int ret;
 
@@ -121,7 +121,7 @@ static const struct ad5592r_rw_ops ad5592r_rw_ops = {
 	.read_adc = ad5592r_read_adc,
 	.reg_write = ad5592r_reg_write,
 	.reg_read = ad5592r_reg_read,
-	.gpio_read = ad5593r_gpio_read,
+	.gpio_read = ad5592r_gpio_read,
 };
 
 static int ad5592r_spi_probe(struct spi_device *spi)
diff --git a/drivers/iio/dac/ad5593r.c b/drivers/iio/dac/ad5593r.c
index 44ea3b8117d0..1fbe9c019c7f 100644
--- a/drivers/iio/dac/ad5593r.c
+++ b/drivers/iio/dac/ad5593r.c
@@ -134,5 +134,5 @@ static struct i2c_driver ad5593r_driver = {
 module_i2c_driver(ad5593r_driver);
 
 MODULE_AUTHOR("Paul Cercueil <paul.cercueil@analog.com>");
-MODULE_DESCRIPTION("Analog Devices AD5592R multi-channel converters");
+MODULE_DESCRIPTION("Analog Devices AD5593R multi-channel converters");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/dac/ad5624r_spi.c b/drivers/iio/dac/ad5624r_spi.c
index e6c022e1dc1c..2015a5df840c 100644
--- a/drivers/iio/dac/ad5624r_spi.c
+++ b/drivers/iio/dac/ad5624r_spi.c
@@ -18,6 +18,8 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+#include <asm/unaligned.h>
+
 #include "ad5624r.h"
 
 static int ad5624r_spi_write(struct spi_device *spi,
@@ -35,11 +37,9 @@ static int ad5624r_spi_write(struct spi_device *spi,
 	 * for the AD5664R, AD5644R, and AD5624R, respectively.
 	 */
 	data = (0 << 22) | (cmd << 19) | (addr << 16) | (val << shift);
-	msg[0] = data >> 16;
-	msg[1] = data >> 8;
-	msg[2] = data;
+	put_unaligned_be24(data, &msg[0]);
 
-	return spi_write(spi, msg, 3);
+	return spi_write(spi, msg, sizeof(msg));
 }
 
 static int ad5624r_read_raw(struct iio_dev *indio_dev,
diff --git a/drivers/iio/dac/ad5686.c b/drivers/iio/dac/ad5686.c
index e06b29c565b9..8dd67da0a7da 100644
--- a/drivers/iio/dac/ad5686.c
+++ b/drivers/iio/dac/ad5686.c
@@ -127,9 +127,9 @@ static int ad5686_read_raw(struct iio_dev *indio_dev,
 
 	switch (m) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		ret = st->read(st, chan->address);
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 		if (ret < 0)
 			return ret;
 		*val = (ret >> chan->scan_type.shift) &
@@ -157,12 +157,12 @@ static int ad5686_write_raw(struct iio_dev *indio_dev,
 		if (val > (1 << chan->scan_type.realbits) || val < 0)
 			return -EINVAL;
 
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&st->lock);
 		ret = st->write(st,
 				AD5686_CMD_WRITE_INPUT_N_UPDATE_N,
 				chan->address,
 				val << chan->scan_type.shift);
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&st->lock);
 		break;
 	default:
 		ret = -EINVAL;
@@ -468,6 +468,8 @@ int ad5686_probe(struct device *dev,
 	indio_dev->channels = st->chip_info->channels;
 	indio_dev->num_channels = st->chip_info->num_channels;
 
+	mutex_init(&st->lock);
+
 	switch (st->chip_info->regmap_type) {
 	case AD5310_REGMAP:
 		cmd = AD5686_CMD_CONTROL_REG;
diff --git a/drivers/iio/dac/ad5686.h b/drivers/iio/dac/ad5686.h
index 70a779939ddb..52009b5eef88 100644
--- a/drivers/iio/dac/ad5686.h
+++ b/drivers/iio/dac/ad5686.h
@@ -117,6 +117,7 @@ struct ad5686_chip_info {
  * @pwr_down_mask:	power down mask
  * @pwr_down_mode:	current power down mode
  * @use_internal_vref:	set to true if the internal reference voltage is used
+ * @lock		lock to protect the data buffer during regmap ops
  * @data:		spi transfer buffers
  */
 
@@ -130,6 +131,7 @@ struct ad5686_state {
 	ad5686_write_func		write;
 	ad5686_read_func		read;
 	bool				use_internal_vref;
+	struct mutex			lock;
 
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
diff --git a/drivers/iio/dac/ad5755.c b/drivers/iio/dac/ad5755.c
index 388ddd14bfd0..7723bd313fc6 100644
--- a/drivers/iio/dac/ad5755.c
+++ b/drivers/iio/dac/ad5755.c
@@ -82,6 +82,7 @@ struct ad5755_chip_info {
  * @pwr_down:	bitmask which contains  hether a channel is powered down or not
  * @ctrl:	software shadow of the channel ctrl registers
  * @channels:	iio channel spec for the device
+ * @lock	lock to protect the data buffer during SPI ops
  * @data:	spi transfer buffers
  */
 struct ad5755_state {
@@ -90,6 +91,7 @@ struct ad5755_state {
 	unsigned int			pwr_down;
 	unsigned int			ctrl[AD5755_NUM_CHANNELS];
 	struct iio_chan_spec		channels[AD5755_NUM_CHANNELS];
+	struct mutex			lock;
 
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
@@ -174,11 +176,12 @@ static int ad5755_write_ctrl_unlocked(struct iio_dev *indio_dev,
 static int ad5755_write(struct iio_dev *indio_dev, unsigned int reg,
 	unsigned int val)
 {
+	struct ad5755_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad5755_write_unlocked(indio_dev, reg, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -186,11 +189,12 @@ static int ad5755_write(struct iio_dev *indio_dev, unsigned int reg,
 static int ad5755_write_ctrl(struct iio_dev *indio_dev, unsigned int channel,
 	unsigned int reg, unsigned int val)
 {
+	struct ad5755_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = ad5755_write_ctrl_unlocked(indio_dev, channel, reg, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -211,7 +215,7 @@ static int ad5755_read(struct iio_dev *indio_dev, unsigned int addr)
 		},
 	};
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->data[0].d32 = cpu_to_be32(AD5755_READ_FLAG | (addr << 16));
 	st->data[1].d32 = cpu_to_be32(AD5755_NOOP);
@@ -220,7 +224,7 @@ static int ad5755_read(struct iio_dev *indio_dev, unsigned int addr)
 	if (ret >= 0)
 		ret = be32_to_cpu(st->data[1].d32) & 0xffff;
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -246,7 +250,7 @@ static int ad5755_set_channel_pwr_down(struct iio_dev *indio_dev,
 	struct ad5755_state *st = iio_priv(indio_dev);
 	unsigned int mask = BIT(channel);
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	if ((bool)(st->pwr_down & mask) == pwr_down)
 		goto out_unlock;
@@ -266,7 +270,7 @@ static int ad5755_set_channel_pwr_down(struct iio_dev *indio_dev,
 	}
 
 out_unlock:
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return 0;
 }
@@ -746,6 +750,8 @@ static int ad5755_probe(struct spi_device *spi)
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->num_channels = AD5755_NUM_CHANNELS;
 
+	mutex_init(&st->lock);
+
 	if (spi->dev.of_node)
 		pdata = ad5755_parse_dt(&spi->dev);
 	else
diff --git a/drivers/iio/dac/ad5761.c b/drivers/iio/dac/ad5761.c
index 7468fbd11684..67c4fa75c6f1 100644
--- a/drivers/iio/dac/ad5761.c
+++ b/drivers/iio/dac/ad5761.c
@@ -57,11 +57,13 @@ enum ad5761_supported_device_ids {
  * @use_intref:		true when the internal voltage reference is used
  * @vref:		actual voltage reference in mVolts
  * @range:		output range mode used
+ * @lock		lock to protect the data buffer during SPI ops
  * @data:		cache aligned spi buffer
  */
 struct ad5761_state {
 	struct spi_device		*spi;
 	struct regulator		*vref_reg;
+	struct mutex			lock;
 
 	bool use_intref;
 	int vref;
@@ -124,9 +126,9 @@ static int ad5761_spi_write(struct iio_dev *indio_dev, u8 addr, u16 val)
 	struct ad5761_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = _ad5761_spi_write(st, addr, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -163,9 +165,9 @@ static int ad5761_spi_read(struct iio_dev *indio_dev, u8 addr, u16 *val)
 	struct ad5761_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	ret = _ad5761_spi_read(st, addr, val);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -368,6 +370,8 @@ static int ad5761_probe(struct spi_device *spi)
 	if (pdata)
 		voltage_range = pdata->voltage_range;
 
+	mutex_init(&st->lock);
+
 	ret = ad5761_spi_set_range(st, voltage_range);
 	if (ret)
 		goto disable_regulator_err;
diff --git a/drivers/iio/dac/ad5764.c b/drivers/iio/dac/ad5764.c
index f7ab211604a1..5b0f0fe354f6 100644
--- a/drivers/iio/dac/ad5764.c
+++ b/drivers/iio/dac/ad5764.c
@@ -46,6 +46,7 @@ struct ad5764_chip_info {
  * @spi:		spi_device
  * @chip_info:		chip info
  * @vref_reg:		vref supply regulators
+ * @lock		lock to protect the data buffer during SPI ops
  * @data:		spi transfer buffers
  */
 
@@ -53,6 +54,7 @@ struct ad5764_state {
 	struct spi_device		*spi;
 	const struct ad5764_chip_info	*chip_info;
 	struct regulator_bulk_data	vref_reg[2];
+	struct mutex			lock;
 
 	/*
 	 * DMA (thus cache coherency maintenance) requires the
@@ -126,11 +128,11 @@ static int ad5764_write(struct iio_dev *indio_dev, unsigned int reg,
 	struct ad5764_state *st = iio_priv(indio_dev);
 	int ret;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 	st->data[0].d32 = cpu_to_be32((reg << 16) | val);
 
 	ret = spi_write(st->spi, &st->data[0].d8[1], 3);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -151,7 +153,7 @@ static int ad5764_read(struct iio_dev *indio_dev, unsigned int reg,
 		},
 	};
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&st->lock);
 
 	st->data[0].d32 = cpu_to_be32((1 << 23) | (reg << 16));
 
@@ -159,7 +161,7 @@ static int ad5764_read(struct iio_dev *indio_dev, unsigned int reg,
 	if (ret >= 0)
 		*val = be32_to_cpu(st->data[1].d32) & 0xffff;
 
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&st->lock);
 
 	return ret;
 }
@@ -295,6 +297,8 @@ static int ad5764_probe(struct spi_device *spi)
 	indio_dev->num_channels = AD5764_NUM_CHANNELS;
 	indio_dev->channels = st->chip_info->channels;
 
+	mutex_init(&st->lock);
+
 	if (st->chip_info->int_vref == 0) {
 		st->vref_reg[0].supply = "vrefAB";
 		st->vref_reg[1].supply = "vrefCD";
diff --git a/drivers/iio/dac/ltc2632.c b/drivers/iio/dac/ltc2632.c
index 7adc91056aa1..f891311f05cf 100644
--- a/drivers/iio/dac/ltc2632.c
+++ b/drivers/iio/dac/ltc2632.c
@@ -12,6 +12,8 @@
 #include <linux/iio/iio.h>
 #include <linux/regulator/consumer.h>
 
+#include <asm/unaligned.h>
+
 #define LTC2632_CMD_WRITE_INPUT_N               0x0
 #define LTC2632_CMD_UPDATE_DAC_N                0x1
 #define LTC2632_CMD_WRITE_INPUT_N_UPDATE_ALL    0x2
@@ -24,6 +26,7 @@
 /**
  * struct ltc2632_chip_info - chip specific information
  * @channels:		channel spec for the DAC
+ * @num_channels:	DAC channel count of the chip
  * @vref_mv:		internal reference voltage
  */
 struct ltc2632_chip_info {
@@ -53,6 +56,12 @@ enum ltc2632_supported_device_ids {
 	ID_LTC2632H12,
 	ID_LTC2632H10,
 	ID_LTC2632H8,
+	ID_LTC2634L12,
+	ID_LTC2634L10,
+	ID_LTC2634L8,
+	ID_LTC2634H12,
+	ID_LTC2634H10,
+	ID_LTC2634H8,
 	ID_LTC2636L12,
 	ID_LTC2636L10,
 	ID_LTC2636L8,
@@ -75,9 +84,7 @@ static int ltc2632_spi_write(struct spi_device *spi,
 	 * 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;
+	put_unaligned_be24(data, &msg[0]);
 
 	return spi_write(spi, msg, sizeof(msg));
 }
@@ -235,6 +242,36 @@ static const struct ltc2632_chip_info ltc2632_chip_info_tbl[] = {
 		.num_channels	= 2,
 		.vref_mv	= 4096,
 	},
+	[ID_LTC2634L12] = {
+		.channels	= ltc2632x12_channels,
+		.num_channels	= 4,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2634L10] = {
+		.channels	= ltc2632x10_channels,
+		.num_channels	= 4,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2634L8] =  {
+		.channels	= ltc2632x8_channels,
+		.num_channels	= 4,
+		.vref_mv	= 2500,
+	},
+	[ID_LTC2634H12] = {
+		.channels	= ltc2632x12_channels,
+		.num_channels	= 4,
+		.vref_mv	= 4096,
+	},
+	[ID_LTC2634H10] = {
+		.channels	= ltc2632x10_channels,
+		.num_channels	= 4,
+		.vref_mv	= 4096,
+	},
+	[ID_LTC2634H8] =  {
+		.channels	= ltc2632x8_channels,
+		.num_channels	= 4,
+		.vref_mv	= 4096,
+	},
 	[ID_LTC2636L12] = {
 		.channels	= ltc2632x12_channels,
 		.num_channels	= 8,
@@ -356,6 +393,12 @@ static const struct spi_device_id ltc2632_id[] = {
 	{ "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] },
+	{ "ltc2634-l12", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634L12] },
+	{ "ltc2634-l10", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634L10] },
+	{ "ltc2634-l8", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634L8] },
+	{ "ltc2634-h12", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634H12] },
+	{ "ltc2634-h10", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634H10] },
+	{ "ltc2634-h8", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2634H8] },
 	{ "ltc2636-l12", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2636L12] },
 	{ "ltc2636-l10", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2636L10] },
 	{ "ltc2636-l8", (kernel_ulong_t)&ltc2632_chip_info_tbl[ID_LTC2636L8] },
@@ -386,6 +429,24 @@ static const struct of_device_id ltc2632_of_match[] = {
 		.compatible = "lltc,ltc2632-h8",
 		.data = &ltc2632_chip_info_tbl[ID_LTC2632H8]
 	}, {
+		.compatible = "lltc,ltc2634-l12",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634L12]
+	}, {
+		.compatible = "lltc,ltc2634-l10",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634L10]
+	}, {
+		.compatible = "lltc,ltc2634-l8",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634L8]
+	}, {
+		.compatible = "lltc,ltc2634-h12",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634H12]
+	}, {
+		.compatible = "lltc,ltc2634-h10",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634H10]
+	}, {
+		.compatible = "lltc,ltc2634-h8",
+		.data = &ltc2632_chip_info_tbl[ID_LTC2634H8]
+	}, {
 		.compatible = "lltc,ltc2636-l12",
 		.data = &ltc2632_chip_info_tbl[ID_LTC2636L12]
 	}, {
diff --git a/drivers/iio/dac/vf610_dac.c b/drivers/iio/dac/vf610_dac.c
index 7f1e9317c3f3..9417a4a3e22a 100644
--- a/drivers/iio/dac/vf610_dac.c
+++ b/drivers/iio/dac/vf610_dac.c
@@ -36,6 +36,7 @@ struct vf610_dac {
 	struct device *dev;
 	enum vf610_conversion_mode_sel conv_mode;
 	void __iomem *regs;
+	struct mutex lock;
 };
 
 static void vf610_dac_init(struct vf610_dac *info)
@@ -64,7 +65,7 @@ static int vf610_set_conversion_mode(struct iio_dev *indio_dev,
 	struct vf610_dac *info = iio_priv(indio_dev);
 	int val;
 
-	mutex_lock(&indio_dev->mlock);
+	mutex_lock(&info->lock);
 	info->conv_mode = mode;
 	val = readl(info->regs + VF610_DACx_STATCTRL);
 	if (mode)
@@ -72,7 +73,7 @@ static int vf610_set_conversion_mode(struct iio_dev *indio_dev,
 	else
 		val &= ~VF610_DAC_LPEN;
 	writel(val, info->regs + VF610_DACx_STATCTRL);
-	mutex_unlock(&indio_dev->mlock);
+	mutex_unlock(&info->lock);
 
 	return 0;
 }
@@ -147,9 +148,9 @@ static int vf610_write_raw(struct iio_dev *indio_dev,
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
-		mutex_lock(&indio_dev->mlock);
+		mutex_lock(&info->lock);
 		writel(VF610_DAC_DAT0(val), info->regs);
-		mutex_unlock(&indio_dev->mlock);
+		mutex_unlock(&info->lock);
 		return 0;
 
 	default:
@@ -205,6 +206,8 @@ static int vf610_dac_probe(struct platform_device *pdev)
 	indio_dev->channels = vf610_dac_iio_channels;
 	indio_dev->num_channels = ARRAY_SIZE(vf610_dac_iio_channels);
 
+	mutex_init(&info->lock);
+
 	ret = clk_prepare_enable(info->clk);
 	if (ret) {
 		dev_err(&pdev->dev,
diff --git a/drivers/iio/gyro/Kconfig b/drivers/iio/gyro/Kconfig
index 7eaf77707b0b..6daeddf37f60 100644
--- a/drivers/iio/gyro/Kconfig
+++ b/drivers/iio/gyro/Kconfig
@@ -61,7 +61,7 @@ config BMG160
 	help
 	  Say yes here to build support for BOSCH BMG160 Tri-axis Gyro Sensor
 	  driver connected via I2C or SPI. This driver also supports BMI055
-	  gyroscope.
+	  and BMI088 gyroscope.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called bmg160_i2c or bmg160_spi.
diff --git a/drivers/iio/gyro/adis16130.c b/drivers/iio/gyro/adis16130.c
index 79e63c8a2ea8..2a9ec08ec561 100644
--- a/drivers/iio/gyro/adis16130.c
+++ b/drivers/iio/gyro/adis16130.c
@@ -12,6 +12,8 @@
 
 #include <linux/iio/iio.h>
 
+#include <asm/unaligned.h>
+
 #define ADIS16130_CON         0x0
 #define ADIS16130_CON_RD      (1 << 6)
 #define ADIS16130_IOP         0x1
@@ -59,7 +61,7 @@ static int adis16130_spi_read(struct iio_dev *indio_dev, u8 reg_addr, u32 *val)
 
 	ret = spi_sync_transfer(st->us, &xfer, 1);
 	if (ret == 0)
-		*val = (st->buf[1] << 16) | (st->buf[2] << 8) | st->buf[3];
+		*val = get_unaligned_be24(&st->buf[1]);
 	mutex_unlock(&st->buf_lock);
 
 	return ret;
diff --git a/drivers/iio/gyro/adis16136.c b/drivers/iio/gyro/adis16136.c
index a4c967a5fc5c..afdc57af475d 100644
--- a/drivers/iio/gyro/adis16136.c
+++ b/drivers/iio/gyro/adis16136.c
@@ -148,16 +148,14 @@ DEFINE_DEBUGFS_ATTRIBUTE(adis16136_flash_count_fops,
 static int adis16136_debugfs_init(struct iio_dev *indio_dev)
 {
 	struct adis16136 *adis16136 = iio_priv(indio_dev);
+	struct dentry *d = iio_get_debugfs_dentry(indio_dev);
 
 	debugfs_create_file_unsafe("serial_number", 0400,
-		indio_dev->debugfs_dentry, adis16136,
-		&adis16136_serial_fops);
+		d, adis16136, &adis16136_serial_fops);
 	debugfs_create_file_unsafe("product_id", 0400,
-		indio_dev->debugfs_dentry,
-		adis16136, &adis16136_product_id_fops);
+		d, adis16136, &adis16136_product_id_fops);
 	debugfs_create_file_unsafe("flash_count", 0400,
-		indio_dev->debugfs_dentry,
-		adis16136, &adis16136_flash_count_fops);
+		d, adis16136, &adis16136_flash_count_fops);
 
 	return 0;
 }
diff --git a/drivers/iio/gyro/bmg160_i2c.c b/drivers/iio/gyro/bmg160_i2c.c
index 4fc9c6a3321f..b3fa46bd02cb 100644
--- a/drivers/iio/gyro/bmg160_i2c.c
+++ b/drivers/iio/gyro/bmg160_i2c.c
@@ -21,8 +21,8 @@ static int bmg160_i2c_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &bmg160_regmap_i2c_conf);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
@@ -42,6 +42,7 @@ static int bmg160_i2c_remove(struct i2c_client *client)
 static const struct acpi_device_id bmg160_acpi_match[] = {
 	{"BMG0160", 0},
 	{"BMI055B", 0},
+	{"BMI088B", 0},
 	{},
 };
 
@@ -50,6 +51,7 @@ MODULE_DEVICE_TABLE(acpi, bmg160_acpi_match);
 static const struct i2c_device_id bmg160_i2c_id[] = {
 	{"bmg160", 0},
 	{"bmi055_gyro", 0},
+	{"bmi088_gyro", 0},
 	{}
 };
 
diff --git a/drivers/iio/gyro/bmg160_spi.c b/drivers/iio/gyro/bmg160_spi.c
index 182a59c42507..745962e1e423 100644
--- a/drivers/iio/gyro/bmg160_spi.c
+++ b/drivers/iio/gyro/bmg160_spi.c
@@ -19,8 +19,8 @@ static int bmg160_spi_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &bmg160_regmap_spi_conf);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
@@ -37,6 +37,7 @@ static int bmg160_spi_remove(struct spi_device *spi)
 static const struct spi_device_id bmg160_spi_id[] = {
 	{"bmg160", 0},
 	{"bmi055_gyro", 0},
+	{"bmi088_gyro", 0},
 	{}
 };
 
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index 08cacbbf31e6..7f382aae1dfd 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 enum gyro_3d_channel {
@@ -326,18 +324,13 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&gyro_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&gyro_state->common_attributes);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -361,9 +354,7 @@ static int hid_gyro_3d_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&gyro_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &gyro_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -378,8 +369,7 @@ static int hid_gyro_3d_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_GYRO_3D);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&gyro_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &gyro_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/gyro/mpu3050-i2c.c b/drivers/iio/gyro/mpu3050-i2c.c
index afa8018b9238..ef5bcbc4b45b 100644
--- a/drivers/iio/gyro/mpu3050-i2c.c
+++ b/drivers/iio/gyro/mpu3050-i2c.c
@@ -51,8 +51,8 @@ static int mpu3050_i2c_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &mpu3050_i2c_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
diff --git a/drivers/iio/gyro/st_gyro_buffer.c b/drivers/iio/gyro/st_gyro_buffer.c
index 7465ad62391c..9c92ff7a82be 100644
--- a/drivers/iio/gyro/st_gyro_buffer.c
+++ b/drivers/iio/gyro/st_gyro_buffer.c
@@ -37,8 +37,7 @@ static int st_gyro_buffer_postenable(struct iio_dev *indio_dev)
 	if (err < 0)
 		return err;
 
-	err = st_sensors_set_axis_enable(indio_dev,
-					 (u8)indio_dev->active_scan_mask[0]);
+	err = st_sensors_set_axis_enable(indio_dev, indio_dev->active_scan_mask[0]);
 	if (err < 0)
 		goto st_gyro_buffer_predisable;
 
diff --git a/drivers/iio/gyro/st_gyro_core.c b/drivers/iio/gyro/st_gyro_core.c
index 26c50b24bc08..c8aa051995d3 100644
--- a/drivers/iio/gyro/st_gyro_core.c
+++ b/drivers/iio/gyro/st_gyro_core.c
@@ -460,6 +460,7 @@ EXPORT_SYMBOL(st_gyro_get_settings);
 int st_gyro_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *gdata = iio_priv(indio_dev);
+	struct st_sensors_platform_data *pdata;
 	int err;
 
 	indio_dev->modes = INDIO_DIRECT_MODE;
@@ -477,12 +478,12 @@ int st_gyro_common_probe(struct iio_dev *indio_dev)
 	indio_dev->channels = gdata->sensor_settings->ch;
 	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
 
-	gdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-					&gdata->sensor_settings->fs.fs_avl[0];
+	gdata->current_fullscale = &gdata->sensor_settings->fs.fs_avl[0];
 	gdata->odr = gdata->sensor_settings->odr.odr_avl[0].hz;
 
-	err = st_sensors_init_sensor(indio_dev,
-				(struct st_sensors_platform_data *)&gyro_pdata);
+	pdata = (struct st_sensors_platform_data *)&gyro_pdata;
+
+	err = st_sensors_init_sensor(indio_dev, pdata);
 	if (err < 0)
 		goto st_gyro_power_off;
 
diff --git a/drivers/iio/health/afe4403.c b/drivers/iio/health/afe4403.c
index dc22dc363a99..e9f87e42ff4f 100644
--- a/drivers/iio/health/afe4403.c
+++ b/drivers/iio/health/afe4403.c
@@ -23,6 +23,8 @@
 #include <linux/iio/triggered_buffer.h>
 #include <linux/iio/trigger_consumer.h>
 
+#include <asm/unaligned.h>
+
 #include "afe440x.h"
 
 #define AFE4403_DRIVER_NAME		"afe4403"
@@ -220,13 +222,11 @@ static int afe4403_read(struct afe4403_data *afe, unsigned int reg, u32 *val)
 	if (ret)
 		return ret;
 
-	ret = spi_write_then_read(afe->spi, &reg, 1, rx, 3);
+	ret = spi_write_then_read(afe->spi, &reg, 1, rx, sizeof(rx));
 	if (ret)
 		return ret;
 
-	*val = (rx[0] << 16) |
-		(rx[1] << 8) |
-		(rx[2]);
+	*val = get_unaligned_be24(&rx[0]);
 
 	/* Disable reading from the device */
 	tx[3] = AFE440X_CONTROL0_WRITE;
@@ -322,13 +322,11 @@ static irqreturn_t afe4403_trigger_handler(int irq, void *private)
 			 indio_dev->masklength) {
 		ret = spi_write_then_read(afe->spi,
 					  &afe4403_channel_values[bit], 1,
-					  rx, 3);
+					  rx, sizeof(rx));
 		if (ret)
 			goto err;
 
-		buffer[i++] = (rx[0] << 16) |
-				(rx[1] << 8) |
-				(rx[2]);
+		buffer[i++] = get_unaligned_be24(&rx[0]);
 	}
 
 	/* Disable reading from the device */
diff --git a/drivers/iio/health/max30100.c b/drivers/iio/health/max30100.c
index 84010501762d..546fc37ad75d 100644
--- a/drivers/iio/health/max30100.c
+++ b/drivers/iio/health/max30100.c
@@ -16,7 +16,7 @@
 #include <linux/irq.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
-#include <linux/of.h>
+#include <linux/property.h>
 #include <linux/regmap.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/buffer.h>
@@ -267,11 +267,10 @@ static int max30100_get_current_idx(unsigned int val, int *reg)
 static int max30100_led_init(struct max30100_data *data)
 {
 	struct device *dev = &data->client->dev;
-	struct device_node *np = dev->of_node;
 	unsigned int val[2];
 	int reg, ret;
 
-	ret = of_property_read_u32_array(np, "maxim,led-current-microamp",
+	ret = device_property_read_u32_array(dev, "maxim,led-current-microamp",
 					(unsigned int *) &val, 2);
 	if (ret) {
 		/* Default to 24 mA RED LED, 50 mA IR LED */
@@ -502,7 +501,7 @@ MODULE_DEVICE_TABLE(of, max30100_dt_ids);
 static struct i2c_driver max30100_driver = {
 	.driver = {
 		.name	= MAX30100_DRV_NAME,
-		.of_match_table	= of_match_ptr(max30100_dt_ids),
+		.of_match_table	= max30100_dt_ids,
 	},
 	.probe		= max30100_probe,
 	.remove		= max30100_remove,
diff --git a/drivers/iio/humidity/hid-sensor-humidity.c b/drivers/iio/humidity/hid-sensor-humidity.c
index c99b54b0568d..d2318c4aab0f 100644
--- a/drivers/iio/humidity/hid-sensor-humidity.c
+++ b/drivers/iio/humidity/hid-sensor-humidity.c
@@ -7,8 +7,6 @@
 #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>
 
@@ -233,12 +231,8 @@ static int hid_humidity_probe(struct platform_device *pdev)
 	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)
@@ -261,7 +255,7 @@ static int hid_humidity_probe(struct platform_device *pdev)
 error_remove_callback:
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_HUMIDITY);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&humid_st->common_attributes);
+	hid_sensor_remove_trigger(indio_dev, &humid_st->common_attributes);
 	return ret;
 }
 
@@ -274,7 +268,7 @@ static int hid_humidity_remove(struct platform_device *pdev)
 
 	iio_device_unregister(indio_dev);
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_HUMIDITY);
-	hid_sensor_remove_trigger(&humid_st->common_attributes);
+	hid_sensor_remove_trigger(indio_dev, &humid_st->common_attributes);
 
 	return 0;
 }
diff --git a/drivers/iio/humidity/hts221_buffer.c b/drivers/iio/humidity/hts221_buffer.c
index 81d50a861c22..9fb3f33614d4 100644
--- a/drivers/iio/humidity/hts221_buffer.c
+++ b/drivers/iio/humidity/hts221_buffer.c
@@ -74,10 +74,9 @@ static irqreturn_t hts221_trigger_handler_thread(int irq, void *private)
 
 int hts221_allocate_trigger(struct hts221_hw *hw)
 {
+	struct st_sensors_platform_data *pdata = dev_get_platdata(hw->dev);
 	struct iio_dev *iio_dev = iio_priv_to_dev(hw);
 	bool irq_active_low = false, open_drain = false;
-	struct device_node *np = hw->dev->of_node;
-	struct st_sensors_platform_data *pdata;
 	unsigned long irq_type;
 	int err;
 
@@ -106,8 +105,7 @@ int hts221_allocate_trigger(struct hts221_hw *hw)
 	if (err < 0)
 		return err;
 
-	pdata = (struct st_sensors_platform_data *)hw->dev->platform_data;
-	if ((np && of_property_read_bool(np, "drive-open-drain")) ||
+	if (device_property_read_bool(hw->dev, "drive-open-drain") ||
 	    (pdata && pdata->open_drain)) {
 		irq_type |= IRQF_SHARED;
 		open_drain = true;
diff --git a/drivers/iio/humidity/hts221_i2c.c b/drivers/iio/humidity/hts221_i2c.c
index 4272b7030c44..cab39c4756f8 100644
--- a/drivers/iio/humidity/hts221_i2c.c
+++ b/drivers/iio/humidity/hts221_i2c.c
@@ -32,8 +32,8 @@ static int hts221_i2c_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &hts221_i2c_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap %ld\n",
+			PTR_ERR(regmap));
 		return PTR_ERR(regmap);
 	}
 
@@ -63,7 +63,7 @@ static struct i2c_driver hts221_driver = {
 	.driver = {
 		.name = "hts221_i2c",
 		.pm = &hts221_pm_ops,
-		.of_match_table = of_match_ptr(hts221_i2c_of_match),
+		.of_match_table = hts221_i2c_of_match,
 		.acpi_match_table = ACPI_PTR(hts221_acpi_match),
 	},
 	.probe = hts221_i2c_probe,
diff --git a/drivers/iio/humidity/hts221_spi.c b/drivers/iio/humidity/hts221_spi.c
index 055dba8897d2..729e86e433b1 100644
--- a/drivers/iio/humidity/hts221_spi.c
+++ b/drivers/iio/humidity/hts221_spi.c
@@ -31,8 +31,8 @@ static int hts221_spi_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &hts221_spi_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap %ld\n",
+			PTR_ERR(regmap));
 		return PTR_ERR(regmap);
 	}
 
@@ -56,7 +56,7 @@ static struct spi_driver hts221_driver = {
 	.driver = {
 		.name = "hts221_spi",
 		.pm = &hts221_pm_ops,
-		.of_match_table = of_match_ptr(hts221_spi_of_match),
+		.of_match_table = hts221_spi_of_match,
 	},
 	.probe = hts221_spi_probe,
 	.id_table = hts221_spi_id_table,
diff --git a/drivers/iio/imu/Kconfig b/drivers/iio/imu/Kconfig
index 60bb1029e759..fc4123d518bc 100644
--- a/drivers/iio/imu/Kconfig
+++ b/drivers/iio/imu/Kconfig
@@ -29,6 +29,19 @@ config ADIS16460
 	  To compile this driver as a module, choose M here: the module will be
 	  called adis16460.
 
+config ADIS16475
+	tristate "Analog Devices ADIS16475 and similar IMU driver"
+	depends on SPI
+	select IIO_ADIS_LIB
+	select IIO_ADIS_LIB_BUFFER if IIO_BUFFER
+	help
+	  Say yes here to build support for Analog Devices ADIS16470, ADIS16475,
+	  ADIS16477, ADIS16465, ADIS16467, ADIS16500, ADIS16505, ADIS16507 inertial
+	  sensors.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called adis16475.
+
 config ADIS16480
 	tristate "Analog Devices ADIS16480 and similar IMU driver"
 	depends on SPI
diff --git a/drivers/iio/imu/Makefile b/drivers/iio/imu/Makefile
index 5237fd4bc384..88b2c4555230 100644
--- a/drivers/iio/imu/Makefile
+++ b/drivers/iio/imu/Makefile
@@ -6,6 +6,7 @@
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_ADIS16400) += adis16400.o
 obj-$(CONFIG_ADIS16460) += adis16460.o
+obj-$(CONFIG_ADIS16475) += adis16475.o
 obj-$(CONFIG_ADIS16480) += adis16480.o
 
 adis_lib-y += adis.o
diff --git a/drivers/iio/imu/adis.c b/drivers/iio/imu/adis.c
index a8afd01de4f3..c539dfa3b8d3 100644
--- a/drivers/iio/imu/adis.c
+++ b/drivers/iio/imu/adis.c
@@ -223,6 +223,31 @@ int __adis_read_reg(struct adis *adis, unsigned int reg,
 	return ret;
 }
 EXPORT_SYMBOL_GPL(__adis_read_reg);
+/**
+ * __adis_update_bits_base() - ADIS Update bits function - Unlocked version
+ * @adis: The adis device
+ * @reg: The address of the lower of the two registers
+ * @mask: Bitmask to change
+ * @val: Value to be written
+ * @size: Size of the register to update
+ *
+ * Updates the desired bits of @reg in accordance with @mask and @val.
+ */
+int __adis_update_bits_base(struct adis *adis, unsigned int reg, const u32 mask,
+			    const u32 val, u8 size)
+{
+	int ret;
+	u32 __val;
+
+	ret = __adis_read_reg(adis, reg, &__val, size);
+	if (ret)
+		return ret;
+
+	__val = (__val & ~mask) | (val & mask);
+
+	return __adis_write_reg(adis, reg, __val, size);
+}
+EXPORT_SYMBOL_GPL(__adis_update_bits_base);
 
 #ifdef CONFIG_DEBUG_FS
 
@@ -419,7 +444,7 @@ int __adis_initial_startup(struct adis *adis)
 
 	if (prod_id != adis->data->prod_id)
 		dev_warn(&adis->spi->dev,
-			 "Device ID(%u) and product ID(%u) do not match.",
+			 "Device ID(%u) and product ID(%u) do not match.\n",
 			 adis->data->prod_id, prod_id);
 
 	return 0;
diff --git a/drivers/iio/imu/adis16400.c b/drivers/iio/imu/adis16400.c
index 05e70c1c4835..229f2ff98469 100644
--- a/drivers/iio/imu/adis16400.c
+++ b/drivers/iio/imu/adis16400.c
@@ -258,7 +258,7 @@ static int adis16400_show_product_id(void *arg, u64 *val)
 
 	return 0;
 }
-DEFINE_SIMPLE_ATTRIBUTE(adis16400_product_id_fops,
+DEFINE_DEBUGFS_ATTRIBUTE(adis16400_product_id_fops,
 	adis16400_show_product_id, NULL, "%lld\n");
 
 static int adis16400_show_flash_count(void *arg, u64 *val)
@@ -275,23 +275,22 @@ static int adis16400_show_flash_count(void *arg, u64 *val)
 
 	return 0;
 }
-DEFINE_SIMPLE_ATTRIBUTE(adis16400_flash_count_fops,
+DEFINE_DEBUGFS_ATTRIBUTE(adis16400_flash_count_fops,
 	adis16400_show_flash_count, NULL, "%lld\n");
 
 static int adis16400_debugfs_init(struct iio_dev *indio_dev)
 {
 	struct adis16400_state *st = iio_priv(indio_dev);
+	struct dentry *d = iio_get_debugfs_dentry(indio_dev);
 
 	if (st->variant->flags & ADIS16400_HAS_SERIAL_NUMBER)
-		debugfs_create_file("serial_number", 0400,
-			indio_dev->debugfs_dentry, st,
-			&adis16400_serial_number_fops);
+		debugfs_create_file_unsafe("serial_number", 0400,
+				d, st, &adis16400_serial_number_fops);
 	if (st->variant->flags & ADIS16400_HAS_PROD_ID)
-		debugfs_create_file("product_id", 0400,
-			indio_dev->debugfs_dentry, st,
-			&adis16400_product_id_fops);
-	debugfs_create_file("flash_count", 0400, indio_dev->debugfs_dentry,
-		st, &adis16400_flash_count_fops);
+		debugfs_create_file_unsafe("product_id", 0400,
+				d, st, &adis16400_product_id_fops);
+	debugfs_create_file_unsafe("flash_count", 0400,
+			d, st, &adis16400_flash_count_fops);
 
 	return 0;
 }
@@ -1194,7 +1193,7 @@ static int adis16400_probe(struct spi_device *spi)
 		indio_dev->available_scan_masks = st->avail_scan_mask;
 		st->adis.burst = &adis16400_burst;
 		if (st->variant->flags & ADIS16400_BURST_DIAG_STAT)
-			st->adis.burst->extra_len = sizeof(u16);
+			st->adis.burst_extra_len = sizeof(u16);
 	}
 
 	adis16400_data = &st->variant->adis_data;
diff --git a/drivers/iio/imu/adis16460.c b/drivers/iio/imu/adis16460.c
index 0027683d0256..ad20c488a3ba 100644
--- a/drivers/iio/imu/adis16460.c
+++ b/drivers/iio/imu/adis16460.c
@@ -87,8 +87,8 @@ static int adis16460_show_serial_number(void *arg, u64 *val)
 
 	return 0;
 }
-DEFINE_SIMPLE_ATTRIBUTE(adis16460_serial_number_fops,
-	adis16460_show_serial_number, NULL, "0x%.4llx\n");
+DEFINE_DEBUGFS_ATTRIBUTE(adis16460_serial_number_fops,
+		adis16460_show_serial_number, NULL, "0x%.4llx\n");
 
 static int adis16460_show_product_id(void *arg, u64 *val)
 {
@@ -105,8 +105,8 @@ static int adis16460_show_product_id(void *arg, u64 *val)
 
 	return 0;
 }
-DEFINE_SIMPLE_ATTRIBUTE(adis16460_product_id_fops,
-	adis16460_show_product_id, NULL, "%llu\n");
+DEFINE_DEBUGFS_ATTRIBUTE(adis16460_product_id_fops,
+		adis16460_show_product_id, NULL, "%llu\n");
 
 static int adis16460_show_flash_count(void *arg, u64 *val)
 {
@@ -123,19 +123,20 @@ static int adis16460_show_flash_count(void *arg, u64 *val)
 
 	return 0;
 }
-DEFINE_SIMPLE_ATTRIBUTE(adis16460_flash_count_fops,
-	adis16460_show_flash_count, NULL, "%lld\n");
+DEFINE_DEBUGFS_ATTRIBUTE(adis16460_flash_count_fops,
+		adis16460_show_flash_count, NULL, "%lld\n");
 
 static int adis16460_debugfs_init(struct iio_dev *indio_dev)
 {
 	struct adis16460 *adis16460 = iio_priv(indio_dev);
-
-	debugfs_create_file("serial_number", 0400, indio_dev->debugfs_dentry,
-		adis16460, &adis16460_serial_number_fops);
-	debugfs_create_file("product_id", 0400, indio_dev->debugfs_dentry,
-		adis16460, &adis16460_product_id_fops);
-	debugfs_create_file("flash_count", 0400, indio_dev->debugfs_dentry,
-		adis16460, &adis16460_flash_count_fops);
+	struct dentry *d = iio_get_debugfs_dentry(indio_dev);
+
+	debugfs_create_file_unsafe("serial_number", 0400,
+			d, adis16460, &adis16460_serial_number_fops);
+	debugfs_create_file_unsafe("product_id", 0400,
+			d, adis16460, &adis16460_product_id_fops);
+	debugfs_create_file_unsafe("flash_count", 0400,
+			d, adis16460, &adis16460_flash_count_fops);
 
 	return 0;
 }
diff --git a/drivers/iio/imu/adis16475.c b/drivers/iio/imu/adis16475.c
new file mode 100644
index 000000000000..c6dac4fc67a1
--- /dev/null
+++ b/drivers/iio/imu/adis16475.c
@@ -0,0 +1,1338 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * ADIS16475 IMU driver
+ *
+ * Copyright 2019 Analog Devices Inc.
+ */
+#include <linux/bitfield.h>
+#include <linux/bitops.h>
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/kernel.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/imu/adis.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/irq.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/property.h>
+#include <linux/spi/spi.h>
+
+#define ADIS16475_REG_DIAG_STAT		0x02
+#define ADIS16475_REG_X_GYRO_L		0x04
+#define ADIS16475_REG_Y_GYRO_L		0x08
+#define ADIS16475_REG_Z_GYRO_L		0x0C
+#define ADIS16475_REG_X_ACCEL_L		0x10
+#define ADIS16475_REG_Y_ACCEL_L		0x14
+#define ADIS16475_REG_Z_ACCEL_L		0x18
+#define ADIS16475_REG_TEMP_OUT		0x1c
+#define ADIS16475_REG_X_GYRO_BIAS_L	0x40
+#define ADIS16475_REG_Y_GYRO_BIAS_L	0x44
+#define ADIS16475_REG_Z_GYRO_BIAS_L	0x48
+#define ADIS16475_REG_X_ACCEL_BIAS_L	0x4c
+#define ADIS16475_REG_Y_ACCEL_BIAS_L	0x50
+#define ADIS16475_REG_Z_ACCEL_BIAS_L	0x54
+#define ADIS16475_REG_FILT_CTRL		0x5c
+#define ADIS16475_FILT_CTRL_MASK	GENMASK(2, 0)
+#define ADIS16475_FILT_CTRL(x)		FIELD_PREP(ADIS16475_FILT_CTRL_MASK, x)
+#define ADIS16475_REG_MSG_CTRL		0x60
+#define ADIS16475_MSG_CTRL_DR_POL_MASK	BIT(0)
+#define ADIS16475_MSG_CTRL_DR_POL(x) \
+				FIELD_PREP(ADIS16475_MSG_CTRL_DR_POL_MASK, x)
+#define ADIS16475_SYNC_MODE_MASK	GENMASK(4, 2)
+#define ADIS16475_SYNC_MODE(x)		FIELD_PREP(ADIS16475_SYNC_MODE_MASK, x)
+#define ADIS16475_REG_UP_SCALE		0x62
+#define ADIS16475_REG_DEC_RATE		0x64
+#define ADIS16475_REG_GLOB_CMD		0x68
+#define ADIS16475_REG_FIRM_REV		0x6c
+#define ADIS16475_REG_FIRM_DM		0x6e
+#define ADIS16475_REG_FIRM_Y		0x70
+#define ADIS16475_REG_PROD_ID		0x72
+#define ADIS16475_REG_SERIAL_NUM	0x74
+#define ADIS16475_REG_FLASH_CNT		0x7c
+#define ADIS16500_BURST32_MASK		BIT(9)
+#define ADIS16500_BURST32(x)		FIELD_PREP(ADIS16500_BURST32_MASK, x)
+/* number of data elements in burst mode */
+#define ADIS16475_BURST32_MAX_DATA	32
+#define ADIS16475_BURST_MAX_DATA	20
+#define ADIS16475_MAX_SCAN_DATA		20
+/* spi max speed in brust mode */
+#define ADIS16475_BURST_MAX_SPEED	1000000
+#define ADIS16475_LSB_DEC_MASK		BIT(0)
+#define ADIS16475_LSB_FIR_MASK		BIT(1)
+
+enum {
+	ADIS16475_SYNC_DIRECT = 1,
+	ADIS16475_SYNC_SCALED,
+	ADIS16475_SYNC_OUTPUT,
+	ADIS16475_SYNC_PULSE = 5,
+};
+
+struct adis16475_sync {
+	u16 sync_mode;
+	u16 min_rate;
+	u16 max_rate;
+};
+
+struct adis16475_chip_info {
+	const struct iio_chan_spec *channels;
+	const struct adis16475_sync *sync;
+	const struct adis_data adis_data;
+	const char *name;
+	u32 num_channels;
+	u32 gyro_max_val;
+	u32 gyro_max_scale;
+	u32 accel_max_val;
+	u32 accel_max_scale;
+	u32 temp_scale;
+	u32 int_clk;
+	u16 max_dec;
+	u8 num_sync;
+	bool has_burst32;
+};
+
+struct adis16475 {
+	const struct adis16475_chip_info *info;
+	struct adis adis;
+	u32 clk_freq;
+	bool burst32;
+	unsigned long lsb_flag;
+	/* Alignment needed for the timestamp */
+	__be16 data[ADIS16475_MAX_SCAN_DATA] __aligned(8);
+};
+
+enum {
+	ADIS16475_SCAN_GYRO_X,
+	ADIS16475_SCAN_GYRO_Y,
+	ADIS16475_SCAN_GYRO_Z,
+	ADIS16475_SCAN_ACCEL_X,
+	ADIS16475_SCAN_ACCEL_Y,
+	ADIS16475_SCAN_ACCEL_Z,
+	ADIS16475_SCAN_TEMP,
+	ADIS16475_SCAN_DIAG_S_FLAGS,
+	ADIS16475_SCAN_CRC_FAILURE,
+};
+
+#ifdef CONFIG_DEBUG_FS
+static ssize_t adis16475_show_firmware_revision(struct file *file,
+						char __user *userbuf,
+						size_t count, loff_t *ppos)
+{
+	struct adis16475 *st = file->private_data;
+	char buf[7];
+	size_t len;
+	u16 rev;
+	int ret;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_REV, &rev);
+	if (ret)
+		return ret;
+
+	len = scnprintf(buf, sizeof(buf), "%x.%x\n", rev >> 8, rev & 0xff);
+
+	return simple_read_from_buffer(userbuf, count, ppos, buf, len);
+}
+
+static const struct file_operations adis16475_firmware_revision_fops = {
+	.open = simple_open,
+	.read = adis16475_show_firmware_revision,
+	.llseek = default_llseek,
+	.owner = THIS_MODULE,
+};
+
+static ssize_t adis16475_show_firmware_date(struct file *file,
+					    char __user *userbuf,
+					    size_t count, loff_t *ppos)
+{
+	struct adis16475 *st = file->private_data;
+	u16 md, year;
+	char buf[12];
+	size_t len;
+	int ret;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_Y, &year);
+	if (ret)
+		return ret;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FIRM_DM, &md);
+	if (ret)
+		return ret;
+
+	len = snprintf(buf, sizeof(buf), "%.2x-%.2x-%.4x\n", md >> 8, md & 0xff,
+		       year);
+
+	return simple_read_from_buffer(userbuf, count, ppos, buf, len);
+}
+
+static const struct file_operations adis16475_firmware_date_fops = {
+	.open = simple_open,
+	.read = adis16475_show_firmware_date,
+	.llseek = default_llseek,
+	.owner = THIS_MODULE,
+};
+
+static int adis16475_show_serial_number(void *arg, u64 *val)
+{
+	struct adis16475 *st = arg;
+	u16 serial;
+	int ret;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_SERIAL_NUM, &serial);
+	if (ret)
+		return ret;
+
+	*val = serial;
+
+	return 0;
+}
+DEFINE_DEBUGFS_ATTRIBUTE(adis16475_serial_number_fops,
+			 adis16475_show_serial_number, NULL, "0x%.4llx\n");
+
+static int adis16475_show_product_id(void *arg, u64 *val)
+{
+	struct adis16475 *st = arg;
+	u16 prod_id;
+	int ret;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_PROD_ID, &prod_id);
+	if (ret)
+		return ret;
+
+	*val = prod_id;
+
+	return 0;
+}
+DEFINE_DEBUGFS_ATTRIBUTE(adis16475_product_id_fops,
+			 adis16475_show_product_id, NULL, "%llu\n");
+
+static int adis16475_show_flash_count(void *arg, u64 *val)
+{
+	struct adis16475 *st = arg;
+	u32 flash_count;
+	int ret;
+
+	ret = adis_read_reg_32(&st->adis, ADIS16475_REG_FLASH_CNT,
+			       &flash_count);
+	if (ret)
+		return ret;
+
+	*val = flash_count;
+
+	return 0;
+}
+DEFINE_DEBUGFS_ATTRIBUTE(adis16475_flash_count_fops,
+			 adis16475_show_flash_count, NULL, "%lld\n");
+
+static void adis16475_debugfs_init(struct iio_dev *indio_dev)
+{
+	struct adis16475 *st = iio_priv(indio_dev);
+	struct dentry *d = iio_get_debugfs_dentry(indio_dev);
+
+	debugfs_create_file_unsafe("serial_number", 0400,
+				   d, st, &adis16475_serial_number_fops);
+	debugfs_create_file_unsafe("product_id", 0400,
+				   d, st, &adis16475_product_id_fops);
+	debugfs_create_file_unsafe("flash_count", 0400,
+				   d, st, &adis16475_flash_count_fops);
+	debugfs_create_file("firmware_revision", 0400,
+			    d, st, &adis16475_firmware_revision_fops);
+	debugfs_create_file("firmware_date", 0400, d,
+			    st, &adis16475_firmware_date_fops);
+}
+#else
+static void adis16475_debugfs_init(struct iio_dev *indio_dev)
+{
+}
+#endif
+
+static int adis16475_get_freq(struct adis16475 *st, u32 *freq)
+{
+	int ret;
+	u16 dec;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_DEC_RATE, &dec);
+	if (ret)
+		return -EINVAL;
+
+	*freq = DIV_ROUND_CLOSEST(st->clk_freq, dec + 1);
+
+	return 0;
+}
+
+static int adis16475_set_freq(struct adis16475 *st, const u32 freq)
+{
+	u16 dec;
+	int ret;
+
+	if (!freq)
+		return -EINVAL;
+
+	dec = DIV_ROUND_CLOSEST(st->clk_freq, freq);
+
+	if (dec)
+		dec--;
+
+	if (dec > st->info->max_dec)
+		dec = st->info->max_dec;
+
+	ret = adis_write_reg_16(&st->adis, ADIS16475_REG_DEC_RATE, dec);
+	if (ret)
+		return ret;
+
+	/*
+	 * If decimation is used, then gyro and accel data will have meaningful
+	 * bits on the LSB registers. This info is used on the trigger handler.
+	 */
+	assign_bit(ADIS16475_LSB_DEC_MASK, &st->lsb_flag, dec);
+
+	return 0;
+}
+
+/* The values are approximated. */
+static const u32 adis16475_3db_freqs[] = {
+	[0] = 720, /* Filter disabled, full BW (~720Hz) */
+	[1] = 360,
+	[2] = 164,
+	[3] = 80,
+	[4] = 40,
+	[5] = 20,
+	[6] = 10,
+};
+
+static int adis16475_get_filter(struct adis16475 *st, u32 *filter)
+{
+	u16 filter_sz;
+	int ret;
+	const int mask = ADIS16475_FILT_CTRL_MASK;
+
+	ret = adis_read_reg_16(&st->adis, ADIS16475_REG_FILT_CTRL, &filter_sz);
+	if (ret)
+		return ret;
+
+	*filter = adis16475_3db_freqs[filter_sz & mask];
+
+	return 0;
+}
+
+static int adis16475_set_filter(struct adis16475 *st, const u32 filter)
+{
+	int i = ARRAY_SIZE(adis16475_3db_freqs);
+	int ret;
+
+	while (--i) {
+		if (adis16475_3db_freqs[i] >= filter)
+			break;
+	}
+
+	ret = adis_write_reg_16(&st->adis, ADIS16475_REG_FILT_CTRL,
+				ADIS16475_FILT_CTRL(i));
+	if (ret)
+		return ret;
+
+	/*
+	 * If FIR is used, then gyro and accel data will have meaningful
+	 * bits on the LSB registers. This info is used on the trigger handler.
+	 */
+	assign_bit(ADIS16475_LSB_FIR_MASK, &st->lsb_flag, i);
+
+	return 0;
+}
+
+static const u32 adis16475_calib_regs[] = {
+	[ADIS16475_SCAN_GYRO_X] = ADIS16475_REG_X_GYRO_BIAS_L,
+	[ADIS16475_SCAN_GYRO_Y] = ADIS16475_REG_Y_GYRO_BIAS_L,
+	[ADIS16475_SCAN_GYRO_Z] = ADIS16475_REG_Z_GYRO_BIAS_L,
+	[ADIS16475_SCAN_ACCEL_X] = ADIS16475_REG_X_ACCEL_BIAS_L,
+	[ADIS16475_SCAN_ACCEL_Y] = ADIS16475_REG_Y_ACCEL_BIAS_L,
+	[ADIS16475_SCAN_ACCEL_Z] = ADIS16475_REG_Z_ACCEL_BIAS_L,
+};
+
+static int adis16475_read_raw(struct iio_dev *indio_dev,
+			      const struct iio_chan_spec *chan,
+			      int *val, int *val2, long info)
+{
+	struct adis16475 *st = iio_priv(indio_dev);
+	int ret;
+	u32 tmp;
+
+	switch (info) {
+	case IIO_CHAN_INFO_RAW:
+		return adis_single_conversion(indio_dev, chan, 0, val);
+	case IIO_CHAN_INFO_SCALE:
+		switch (chan->type) {
+		case IIO_ANGL_VEL:
+			*val = st->info->gyro_max_val;
+			*val2 = st->info->gyro_max_scale;
+			return IIO_VAL_FRACTIONAL;
+		case IIO_ACCEL:
+			*val = st->info->accel_max_val;
+			*val2 = st->info->accel_max_scale;
+			return IIO_VAL_FRACTIONAL;
+		case IIO_TEMP:
+			*val = st->info->temp_scale;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_CALIBBIAS:
+		ret = adis_read_reg_32(&st->adis,
+				       adis16475_calib_regs[chan->scan_index],
+				       val);
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
+		ret = adis16475_get_filter(st, val);
+		if (ret)
+			return ret;
+
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		ret = adis16475_get_freq(st, &tmp);
+		if (ret)
+			return ret;
+
+		*val = tmp / 1000;
+		*val2 = (tmp % 1000) * 1000;
+		return IIO_VAL_INT_PLUS_MICRO;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int adis16475_write_raw(struct iio_dev *indio_dev,
+			       const struct iio_chan_spec *chan,
+			       int val, int val2, long info)
+{
+	struct adis16475 *st = iio_priv(indio_dev);
+	u32 tmp;
+
+	switch (info) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		tmp = val * 1000 + val2 / 1000;
+		return adis16475_set_freq(st, tmp);
+	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
+		return adis16475_set_filter(st, val);
+	case IIO_CHAN_INFO_CALIBBIAS:
+		return adis_write_reg_32(&st->adis,
+					 adis16475_calib_regs[chan->scan_index],
+					 val);
+	default:
+		return -EINVAL;
+	}
+}
+
+#define ADIS16475_MOD_CHAN(_type, _mod, _address, _si, _r_bits, _s_bits) \
+	{ \
+		.type = (_type), \
+		.modified = 1, \
+		.channel2 = (_mod), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_CALIBBIAS), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE), \
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+			BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
+		.address = (_address), \
+		.scan_index = (_si), \
+		.scan_type = { \
+			.sign = 's', \
+			.realbits = (_r_bits), \
+			.storagebits = (_s_bits), \
+			.endianness = IIO_BE, \
+		}, \
+	}
+
+#define ADIS16475_GYRO_CHANNEL(_mod) \
+	ADIS16475_MOD_CHAN(IIO_ANGL_VEL, IIO_MOD_ ## _mod, \
+			   ADIS16475_REG_ ## _mod ## _GYRO_L, \
+			   ADIS16475_SCAN_GYRO_ ## _mod, 32, 32)
+
+#define ADIS16475_ACCEL_CHANNEL(_mod) \
+	ADIS16475_MOD_CHAN(IIO_ACCEL, IIO_MOD_ ## _mod, \
+			   ADIS16475_REG_ ## _mod ## _ACCEL_L, \
+			   ADIS16475_SCAN_ACCEL_ ## _mod, 32, 32)
+
+#define ADIS16475_TEMP_CHANNEL() { \
+		.type = IIO_TEMP, \
+		.indexed = 1, \
+		.channel = 0, \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) | \
+			BIT(IIO_CHAN_INFO_SCALE), \
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ) | \
+			BIT(IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY), \
+		.address = ADIS16475_REG_TEMP_OUT, \
+		.scan_index = ADIS16475_SCAN_TEMP, \
+		.scan_type = { \
+			.sign = 's', \
+			.realbits = 16, \
+			.storagebits = 16, \
+			.endianness = IIO_BE, \
+		}, \
+	}
+
+static const struct iio_chan_spec adis16475_channels[] = {
+	ADIS16475_GYRO_CHANNEL(X),
+	ADIS16475_GYRO_CHANNEL(Y),
+	ADIS16475_GYRO_CHANNEL(Z),
+	ADIS16475_ACCEL_CHANNEL(X),
+	ADIS16475_ACCEL_CHANNEL(Y),
+	ADIS16475_ACCEL_CHANNEL(Z),
+	ADIS16475_TEMP_CHANNEL(),
+	IIO_CHAN_SOFT_TIMESTAMP(7)
+};
+
+enum adis16475_variant {
+	ADIS16470,
+	ADIS16475_1,
+	ADIS16475_2,
+	ADIS16475_3,
+	ADIS16477_1,
+	ADIS16477_2,
+	ADIS16477_3,
+	ADIS16465_1,
+	ADIS16465_2,
+	ADIS16465_3,
+	ADIS16467_1,
+	ADIS16467_2,
+	ADIS16467_3,
+	ADIS16500,
+	ADIS16505_1,
+	ADIS16505_2,
+	ADIS16505_3,
+	ADIS16507_1,
+	ADIS16507_2,
+	ADIS16507_3,
+};
+
+enum {
+	ADIS16475_DIAG_STAT_DATA_PATH = 1,
+	ADIS16475_DIAG_STAT_FLASH_MEM,
+	ADIS16475_DIAG_STAT_SPI,
+	ADIS16475_DIAG_STAT_STANDBY,
+	ADIS16475_DIAG_STAT_SENSOR,
+	ADIS16475_DIAG_STAT_MEMORY,
+	ADIS16475_DIAG_STAT_CLK,
+};
+
+static const char * const adis16475_status_error_msgs[] = {
+	[ADIS16475_DIAG_STAT_DATA_PATH] = "Data Path Overrun",
+	[ADIS16475_DIAG_STAT_FLASH_MEM] = "Flash memory update failure",
+	[ADIS16475_DIAG_STAT_SPI] = "SPI communication error",
+	[ADIS16475_DIAG_STAT_STANDBY] = "Standby mode",
+	[ADIS16475_DIAG_STAT_SENSOR] = "Sensor failure",
+	[ADIS16475_DIAG_STAT_MEMORY] = "Memory failure",
+	[ADIS16475_DIAG_STAT_CLK] = "Clock error",
+};
+
+static int adis16475_enable_irq(struct adis *adis, bool enable)
+{
+	/*
+	 * There is no way to gate the data-ready signal internally inside the
+	 * ADIS16475. We can only control it's polarity...
+	 */
+	if (enable)
+		enable_irq(adis->spi->irq);
+	else
+		disable_irq(adis->spi->irq);
+
+	return 0;
+}
+
+#define ADIS16475_DATA(_prod_id, _timeouts)				\
+{									\
+	.msc_ctrl_reg = ADIS16475_REG_MSG_CTRL,				\
+	.glob_cmd_reg = ADIS16475_REG_GLOB_CMD,				\
+	.diag_stat_reg = ADIS16475_REG_DIAG_STAT,			\
+	.prod_id_reg = ADIS16475_REG_PROD_ID,				\
+	.prod_id = (_prod_id),						\
+	.self_test_mask = BIT(2),					\
+	.self_test_reg = ADIS16475_REG_GLOB_CMD,			\
+	.cs_change_delay = 16,						\
+	.read_delay = 5,						\
+	.write_delay = 5,						\
+	.status_error_msgs = adis16475_status_error_msgs,		\
+	.status_error_mask = BIT(ADIS16475_DIAG_STAT_DATA_PATH) |	\
+		BIT(ADIS16475_DIAG_STAT_FLASH_MEM) |			\
+		BIT(ADIS16475_DIAG_STAT_SPI) |				\
+		BIT(ADIS16475_DIAG_STAT_STANDBY) |			\
+		BIT(ADIS16475_DIAG_STAT_SENSOR) |			\
+		BIT(ADIS16475_DIAG_STAT_MEMORY) |			\
+		BIT(ADIS16475_DIAG_STAT_CLK),				\
+	.enable_irq = adis16475_enable_irq,				\
+	.timeouts = (_timeouts),					\
+}
+
+static const struct adis16475_sync adis16475_sync_mode[] = {
+	{ ADIS16475_SYNC_OUTPUT },
+	{ ADIS16475_SYNC_DIRECT, 1900, 2100 },
+	{ ADIS16475_SYNC_SCALED, 1, 128 },
+	{ ADIS16475_SYNC_PULSE, 1000, 2100 },
+};
+
+static const struct adis_timeout adis16475_timeouts = {
+	.reset_ms = 200,
+	.sw_reset_ms = 200,
+	.self_test_ms = 20,
+};
+
+static const struct adis_timeout adis1650x_timeouts = {
+	.reset_ms = 260,
+	.sw_reset_ms = 260,
+	.self_test_ms = 30,
+};
+
+static const struct adis16475_chip_info adis16475_chip_info[] = {
+	[ADIS16470] = {
+		.name = "adis16470",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16470, &adis16475_timeouts),
+	},
+	[ADIS16475_1] = {
+		.name = "adis16475-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16475, &adis16475_timeouts),
+	},
+	[ADIS16475_2] = {
+		.name = "adis16475-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16475, &adis16475_timeouts),
+	},
+	[ADIS16475_3] = {
+		.name = "adis16475-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16475, &adis16475_timeouts),
+	},
+	[ADIS16477_1] = {
+		.name = "adis16477-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16477, &adis16475_timeouts),
+	},
+	[ADIS16477_2] = {
+		.name = "adis16477-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16477, &adis16475_timeouts),
+	},
+	[ADIS16477_3] = {
+		.name = "adis16477-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16477, &adis16475_timeouts),
+	},
+	[ADIS16465_1] = {
+		.name = "adis16465-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16465, &adis16475_timeouts),
+	},
+	[ADIS16465_2] = {
+		.name = "adis16465-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16465, &adis16475_timeouts),
+	},
+	[ADIS16465_3] = {
+		.name = "adis16465-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(4000 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16465, &adis16475_timeouts),
+	},
+	[ADIS16467_1] = {
+		.name = "adis16467-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16467, &adis16475_timeouts),
+	},
+	[ADIS16467_2] = {
+		.name = "adis16467-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16467, &adis16475_timeouts),
+	},
+	[ADIS16467_3] = {
+		.name = "adis16467-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 1,
+		.accel_max_scale = IIO_M_S_2_TO_G(800 << 16),
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode),
+		.adis_data = ADIS16475_DATA(16467, &adis16475_timeouts),
+	},
+	[ADIS16500] = {
+		.name = "adis16500",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 392,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16500, &adis1650x_timeouts),
+	},
+	[ADIS16505_1] = {
+		.name = "adis16505-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 78,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts),
+	},
+	[ADIS16505_2] = {
+		.name = "adis16505-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 78,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts),
+	},
+	[ADIS16505_3] = {
+		.name = "adis16505-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 78,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16505, &adis1650x_timeouts),
+	},
+	[ADIS16507_1] = {
+		.name = "adis16507-1",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(160 << 16),
+		.accel_max_val = 392,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts),
+	},
+	[ADIS16507_2] = {
+		.name = "adis16507-2",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(40 << 16),
+		.accel_max_val = 392,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts),
+	},
+	[ADIS16507_3] = {
+		.name = "adis16507-3",
+		.num_channels = ARRAY_SIZE(adis16475_channels),
+		.channels = adis16475_channels,
+		.gyro_max_val = 1,
+		.gyro_max_scale = IIO_RAD_TO_DEGREE(10 << 16),
+		.accel_max_val = 392,
+		.accel_max_scale = 32000 << 16,
+		.temp_scale = 100,
+		.int_clk = 2000,
+		.max_dec = 1999,
+		.sync = adis16475_sync_mode,
+		/* pulse sync not supported */
+		.num_sync = ARRAY_SIZE(adis16475_sync_mode) - 1,
+		.has_burst32 = true,
+		.adis_data = ADIS16475_DATA(16507, &adis1650x_timeouts),
+	},
+};
+
+static const struct iio_info adis16475_info = {
+	.read_raw = &adis16475_read_raw,
+	.write_raw = &adis16475_write_raw,
+	.update_scan_mode = adis_update_scan_mode,
+	.debugfs_reg_access = adis_debugfs_reg_access,
+};
+
+static struct adis_burst adis16475_burst = {
+	.en = true,
+	.reg_cmd = ADIS16475_REG_GLOB_CMD,
+	/*
+	 * adis_update_scan_mode_burst() sets the burst length in respect with
+	 * the number of channels and allocates 16 bits for each. However,
+	 * adis1647x devices also need space for DIAG_STAT, DATA_CNTR or
+	 * TIME_STAMP (depending on the clock mode but for us these bytes are
+	 * don't care...) and CRC.
+	 */
+	.extra_len = 3 * sizeof(u16),
+	.burst_max_len = ADIS16475_BURST32_MAX_DATA,
+};
+
+static bool adis16475_validate_crc(const u8 *buffer, u16 crc,
+				   const bool burst32)
+{
+	int i;
+	/* extra 6 elements for low gyro and accel */
+	const u16 sz = burst32 ? ADIS16475_BURST32_MAX_DATA :
+		ADIS16475_BURST_MAX_DATA;
+
+	for (i = 0; i < sz - 2; i++)
+		crc -= buffer[i];
+
+	return crc == 0;
+}
+
+static void adis16475_burst32_check(struct adis16475 *st)
+{
+	int ret;
+	struct adis *adis = &st->adis;
+
+	if (!st->info->has_burst32)
+		return;
+
+	if (st->lsb_flag && !st->burst32) {
+		const u16 en = ADIS16500_BURST32(1);
+
+		ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL,
+					 ADIS16500_BURST32_MASK, en);
+		if (ret)
+			return;
+
+		st->burst32 = true;
+
+		/*
+		 * In 32-bit mode we need extra 2 bytes for all gyro
+		 * and accel channels.
+		 */
+		adis->burst_extra_len = 6 * sizeof(u16);
+		adis->xfer[1].len += 6 * sizeof(u16);
+		dev_dbg(&adis->spi->dev, "Enable burst32 mode, xfer:%d",
+			adis->xfer[1].len);
+
+	} else if (!st->lsb_flag && st->burst32) {
+		const u16 en = ADIS16500_BURST32(0);
+
+		ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL,
+					 ADIS16500_BURST32_MASK, en);
+		if (ret)
+			return;
+
+		st->burst32 = false;
+
+		/* Remove the extra bits */
+		adis->burst_extra_len = 0;
+		adis->xfer[1].len -= 6 * sizeof(u16);
+		dev_dbg(&adis->spi->dev, "Disable burst32 mode, xfer:%d\n",
+			adis->xfer[1].len);
+	}
+}
+
+static irqreturn_t adis16475_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct adis16475 *st = iio_priv(indio_dev);
+	struct adis *adis = &st->adis;
+	int ret, bit, i = 0;
+	__be16 *buffer;
+	u16 crc;
+	bool valid;
+	/* offset until the first element after gyro and accel */
+	const u8 offset = st->burst32 ? 13 : 7;
+	const u32 cached_spi_speed_hz = adis->spi->max_speed_hz;
+
+	adis->spi->max_speed_hz = ADIS16475_BURST_MAX_SPEED;
+
+	ret = spi_sync(adis->spi, &adis->msg);
+	if (ret)
+		return ret;
+
+	adis->spi->max_speed_hz = cached_spi_speed_hz;
+	buffer = adis->buffer;
+
+	crc = be16_to_cpu(buffer[offset + 2]);
+	valid = adis16475_validate_crc(adis->buffer, crc, st->burst32);
+	if (!valid) {
+		dev_err(&adis->spi->dev, "Invalid crc\n");
+		goto check_burst32;
+	}
+
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		/*
+		 * When burst mode is used, system flags is the first data
+		 * channel in the sequence, but the scan index is 7.
+		 */
+		switch (bit) {
+		case ADIS16475_SCAN_TEMP:
+			st->data[i++] = buffer[offset];
+			break;
+		case ADIS16475_SCAN_GYRO_X ... ADIS16475_SCAN_ACCEL_Z:
+			/*
+			 * The first 2 bytes on the received data are the
+			 * DIAG_STAT reg, hence the +1 offset here...
+			 */
+			if (st->burst32) {
+				/* upper 16 */
+				st->data[i++] = buffer[bit * 2 + 2];
+				/* lower 16 */
+				st->data[i++] = buffer[bit * 2 + 1];
+			} else {
+				st->data[i++] = buffer[bit + 1];
+				/*
+				 * Don't bother in doing the manual read if the
+				 * device supports burst32. burst32 will be
+				 * enabled in the next call to
+				 * adis16475_burst32_check()...
+				 */
+				if (st->lsb_flag && !st->info->has_burst32) {
+					u16 val = 0;
+					const u32 reg = ADIS16475_REG_X_GYRO_L +
+						bit * 4;
+
+					adis_read_reg_16(adis, reg, &val);
+					st->data[i++] = cpu_to_be16(val);
+				} else {
+					/* lower not used */
+					st->data[i++] = 0;
+				}
+			}
+			break;
+		}
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, st->data, pf->timestamp);
+check_burst32:
+	/*
+	 * We only check the burst mode at the end of the current capture since
+	 * it takes a full data ready cycle for the device to update the burst
+	 * array.
+	 */
+	adis16475_burst32_check(st);
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static void adis16475_disable_clk(void *data)
+{
+	clk_disable_unprepare((struct clk *)data);
+}
+
+static int adis16475_config_sync_mode(struct adis16475 *st)
+{
+	int ret;
+	struct device *dev = &st->adis.spi->dev;
+	const struct adis16475_sync *sync;
+	u32 sync_mode;
+
+	/* default to internal clk */
+	st->clk_freq = st->info->int_clk * 1000;
+
+	ret = device_property_read_u32(dev, "adi,sync-mode", &sync_mode);
+	if (ret)
+		return 0;
+
+	if (sync_mode >= st->info->num_sync) {
+		dev_err(dev, "Invalid sync mode: %u for %s\n", sync_mode,
+			st->info->name);
+		return -EINVAL;
+	}
+
+	sync = &st->info->sync[sync_mode];
+
+	/* All the other modes require external input signal */
+	if (sync->sync_mode != ADIS16475_SYNC_OUTPUT) {
+		struct clk *clk = devm_clk_get(dev, NULL);
+
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+
+		ret = clk_prepare_enable(clk);
+		if (ret)
+			return ret;
+
+		ret = devm_add_action_or_reset(dev, adis16475_disable_clk, clk);
+		if (ret)
+			return ret;
+
+		st->clk_freq = clk_get_rate(clk);
+		if (st->clk_freq < sync->min_rate ||
+		    st->clk_freq > sync->max_rate) {
+			dev_err(dev,
+				"Clk rate:%u not in a valid range:[%u %u]\n",
+				st->clk_freq, sync->min_rate, sync->max_rate);
+			return -EINVAL;
+		}
+
+		if (sync->sync_mode == ADIS16475_SYNC_SCALED) {
+			u16 up_scale;
+			u32 scaled_out_freq = 0;
+			/*
+			 * If we are in scaled mode, we must have an up_scale.
+			 * In scaled mode the allowable input clock range is
+			 * 1 Hz to 128 Hz, and the allowable output range is
+			 * 1900 to 2100 Hz. Hence, a scale must be given to
+			 * get the allowable output.
+			 */
+			ret = device_property_read_u32(dev,
+						       "adi,scaled-output-hz",
+						       &scaled_out_freq);
+			if (ret) {
+				dev_err(dev, "adi,scaled-output-hz must be given when in scaled sync mode");
+				return -EINVAL;
+			} else if (scaled_out_freq < 1900 ||
+				   scaled_out_freq > 2100) {
+				dev_err(dev, "Invalid value: %u for adi,scaled-output-hz",
+					scaled_out_freq);
+				return -EINVAL;
+			}
+
+			up_scale = DIV_ROUND_CLOSEST(scaled_out_freq,
+						     st->clk_freq);
+
+			ret = __adis_write_reg_16(&st->adis,
+						  ADIS16475_REG_UP_SCALE,
+						  up_scale);
+			if (ret)
+				return ret;
+
+			st->clk_freq = scaled_out_freq;
+		}
+
+		st->clk_freq *= 1000;
+	}
+	/*
+	 * Keep in mind that the mask for the clk modes in adis1650*
+	 * chips is different (1100 instead of 11100). However, we
+	 * are not configuring BIT(4) in these chips and the default
+	 * value is 0, so we are fine in doing the below operations.
+	 * I'm keeping this for simplicity and avoiding extra variables
+	 * in chip_info.
+	 */
+	ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL,
+				 ADIS16475_SYNC_MODE_MASK, sync->sync_mode);
+	if (ret)
+		return ret;
+
+	usleep_range(250, 260);
+
+	return 0;
+}
+
+static int adis16475_config_irq_pin(struct adis16475 *st)
+{
+	int ret;
+	struct irq_data *desc;
+	u32 irq_type;
+	u16 val = 0;
+	u8 polarity;
+	struct spi_device *spi = st->adis.spi;
+
+	desc = irq_get_irq_data(spi->irq);
+	if (!desc) {
+		dev_err(&spi->dev, "Could not find IRQ %d\n", spi->irq);
+		return -EINVAL;
+	}
+	/*
+	 * It is possible to configure the data ready polarity. Furthermore, we
+	 * need to update the adis struct if we want data ready as active low.
+	 */
+	irq_type = irqd_get_trigger_type(desc);
+	if (irq_type == IRQ_TYPE_EDGE_RISING) {
+		polarity = 1;
+		st->adis.irq_flag = IRQF_TRIGGER_RISING;
+	} else if (irq_type == IRQ_TYPE_EDGE_FALLING) {
+		polarity = 0;
+		st->adis.irq_flag = IRQF_TRIGGER_FALLING;
+	} else {
+		dev_err(&spi->dev, "Invalid interrupt type 0x%x specified\n",
+			irq_type);
+		return -EINVAL;
+	}
+
+	val = ADIS16475_MSG_CTRL_DR_POL(polarity);
+	ret = __adis_update_bits(&st->adis, ADIS16475_REG_MSG_CTRL,
+				 ADIS16475_MSG_CTRL_DR_POL_MASK, val);
+	if (ret)
+		return ret;
+	/*
+	 * There is a delay writing to any bits written to the MSC_CTRL
+	 * register. It should not be bigger than 200us, so 250 should be more
+	 * than enough!
+	 */
+	usleep_range(250, 260);
+
+	return 0;
+}
+
+static const struct of_device_id adis16475_of_match[] = {
+	{ .compatible = "adi,adis16470",
+		.data = &adis16475_chip_info[ADIS16470] },
+	{ .compatible = "adi,adis16475-1",
+		.data = &adis16475_chip_info[ADIS16475_1] },
+	{ .compatible = "adi,adis16475-2",
+		.data = &adis16475_chip_info[ADIS16475_2] },
+	{ .compatible = "adi,adis16475-3",
+		.data = &adis16475_chip_info[ADIS16475_3] },
+	{ .compatible = "adi,adis16477-1",
+		.data = &adis16475_chip_info[ADIS16477_1] },
+	{ .compatible = "adi,adis16477-2",
+		.data = &adis16475_chip_info[ADIS16477_2] },
+	{ .compatible = "adi,adis16477-3",
+		.data = &adis16475_chip_info[ADIS16477_3] },
+	{ .compatible = "adi,adis16465-1",
+		.data = &adis16475_chip_info[ADIS16465_1] },
+	{ .compatible = "adi,adis16465-2",
+		.data = &adis16475_chip_info[ADIS16465_2] },
+	{ .compatible = "adi,adis16465-3",
+		.data = &adis16475_chip_info[ADIS16465_3] },
+	{ .compatible = "adi,adis16467-1",
+		.data = &adis16475_chip_info[ADIS16467_1] },
+	{ .compatible = "adi,adis16467-2",
+		.data = &adis16475_chip_info[ADIS16467_2] },
+	{ .compatible = "adi,adis16467-3",
+		.data = &adis16475_chip_info[ADIS16467_3] },
+	{ .compatible = "adi,adis16500",
+		.data = &adis16475_chip_info[ADIS16500] },
+	{ .compatible = "adi,adis16505-1",
+		.data = &adis16475_chip_info[ADIS16505_1] },
+	{ .compatible = "adi,adis16505-2",
+		.data = &adis16475_chip_info[ADIS16505_2] },
+	{ .compatible = "adi,adis16505-3",
+		.data = &adis16475_chip_info[ADIS16505_3] },
+	{ .compatible = "adi,adis16507-1",
+		.data = &adis16475_chip_info[ADIS16507_1] },
+	{ .compatible = "adi,adis16507-2",
+		.data = &adis16475_chip_info[ADIS16507_2] },
+	{ .compatible = "adi,adis16507-3",
+		.data = &adis16475_chip_info[ADIS16507_3] },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, adis16475_of_match);
+
+static int adis16475_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct adis16475 *st;
+	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->adis.burst = &adis16475_burst;
+
+	st->info = device_get_match_data(&spi->dev);
+	if (!st->info)
+		return -EINVAL;
+
+	ret = adis_init(&st->adis, indio_dev, spi, &st->info->adis_data);
+	if (ret)
+		return ret;
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = st->info->name;
+	indio_dev->channels = st->info->channels;
+	indio_dev->num_channels = st->info->num_channels;
+	indio_dev->info = &adis16475_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = __adis_initial_startup(&st->adis);
+	if (ret)
+		return ret;
+
+	ret = adis16475_config_irq_pin(st);
+	if (ret)
+		return ret;
+
+	ret = adis16475_config_sync_mode(st);
+	if (ret)
+		return ret;
+
+	ret = devm_adis_setup_buffer_and_trigger(&st->adis, indio_dev,
+						 adis16475_trigger_handler);
+	if (ret)
+		return ret;
+
+	adis16475_enable_irq(&st->adis, false);
+
+	ret = devm_iio_device_register(&spi->dev, indio_dev);
+	if (ret)
+		return ret;
+
+	adis16475_debugfs_init(indio_dev);
+
+	return 0;
+}
+
+static struct spi_driver adis16475_driver = {
+	.driver = {
+		.name = "adis16475",
+		.of_match_table = adis16475_of_match,
+	},
+	.probe = adis16475_probe,
+};
+module_spi_driver(adis16475_driver);
+
+MODULE_AUTHOR("Nuno Sa <nuno.sa@analog.com>");
+MODULE_DESCRIPTION("Analog Devices ADIS16475 IMU driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/imu/adis16480.c b/drivers/iio/imu/adis16480.c
index cfae0e4476e7..6a471eee110e 100644
--- a/drivers/iio/imu/adis16480.c
+++ b/drivers/iio/imu/adis16480.c
@@ -284,22 +284,18 @@ DEFINE_DEBUGFS_ATTRIBUTE(adis16480_flash_count_fops,
 static int adis16480_debugfs_init(struct iio_dev *indio_dev)
 {
 	struct adis16480 *adis16480 = iio_priv(indio_dev);
+	struct dentry *d = iio_get_debugfs_dentry(indio_dev);
 
 	debugfs_create_file_unsafe("firmware_revision", 0400,
-		indio_dev->debugfs_dentry, adis16480,
-		&adis16480_firmware_revision_fops);
+		d, adis16480, &adis16480_firmware_revision_fops);
 	debugfs_create_file_unsafe("firmware_date", 0400,
-		indio_dev->debugfs_dentry, adis16480,
-		&adis16480_firmware_date_fops);
+		d, adis16480, &adis16480_firmware_date_fops);
 	debugfs_create_file_unsafe("serial_number", 0400,
-		indio_dev->debugfs_dentry, adis16480,
-		&adis16480_serial_number_fops);
+		d, adis16480, &adis16480_serial_number_fops);
 	debugfs_create_file_unsafe("product_id", 0400,
-		indio_dev->debugfs_dentry, adis16480,
-		&adis16480_product_id_fops);
+		d, adis16480, &adis16480_product_id_fops);
 	debugfs_create_file_unsafe("flash_count", 0400,
-		indio_dev->debugfs_dentry, adis16480,
-		&adis16480_flash_count_fops);
+		d, adis16480, &adis16480_flash_count_fops);
 
 	return 0;
 }
diff --git a/drivers/iio/imu/adis_buffer.c b/drivers/iio/imu/adis_buffer.c
index 04e5e2a0fd6b..5b4225ee09b9 100644
--- a/drivers/iio/imu/adis_buffer.c
+++ b/drivers/iio/imu/adis_buffer.c
@@ -23,25 +23,30 @@ static int adis_update_scan_mode_burst(struct iio_dev *indio_dev,
 	const unsigned long *scan_mask)
 {
 	struct adis *adis = iio_device_get_drvdata(indio_dev);
-	unsigned int burst_length;
+	unsigned int burst_length, burst_max_length;
 	u8 *tx;
 
 	/* All but the timestamp channel */
 	burst_length = (indio_dev->num_channels - 1) * sizeof(u16);
-	burst_length += adis->burst->extra_len;
+	burst_length += adis->burst->extra_len + adis->burst_extra_len;
+
+	if (adis->burst->burst_max_len)
+		burst_max_length = adis->burst->burst_max_len;
+	else
+		burst_max_length = burst_length;
 
 	adis->xfer = kcalloc(2, sizeof(*adis->xfer), GFP_KERNEL);
 	if (!adis->xfer)
 		return -ENOMEM;
 
-	adis->buffer = kzalloc(burst_length + sizeof(u16), GFP_KERNEL);
+	adis->buffer = kzalloc(burst_max_length + sizeof(u16), GFP_KERNEL);
 	if (!adis->buffer) {
 		kfree(adis->xfer);
 		adis->xfer = NULL;
 		return -ENOMEM;
 	}
 
-	tx = adis->buffer + burst_length;
+	tx = adis->buffer + burst_max_length;
 	tx[0] = ADIS_READ_REG(adis->burst->reg_cmd);
 	tx[1] = 0;
 
@@ -156,6 +161,14 @@ static irqreturn_t adis_trigger_handler(int irq, void *p)
 	return IRQ_HANDLED;
 }
 
+static void adis_buffer_cleanup(void *arg)
+{
+	struct adis *adis = arg;
+
+	kfree(adis->buffer);
+	kfree(adis->xfer);
+}
+
 /**
  * adis_setup_buffer_and_trigger() - Sets up buffer and trigger for the adis device
  * @adis: The adis device.
@@ -199,6 +212,43 @@ error_buffer_cleanup:
 EXPORT_SYMBOL_GPL(adis_setup_buffer_and_trigger);
 
 /**
+ * devm_adis_setup_buffer_and_trigger() - Sets up buffer and trigger for
+ *					  the managed adis device
+ * @adis: The adis device
+ * @indio_dev: The IIO device
+ * @trigger_handler: Optional trigger handler, may be NULL.
+ *
+ * Returns 0 on success, a negative error code otherwise.
+ *
+ * This function perfoms exactly the same as adis_setup_buffer_and_trigger()
+ */
+int
+devm_adis_setup_buffer_and_trigger(struct adis *adis, struct iio_dev *indio_dev,
+				   irq_handler_t trigger_handler)
+{
+	int ret;
+
+	if (!trigger_handler)
+		trigger_handler = adis_trigger_handler;
+
+	ret = devm_iio_triggered_buffer_setup(&adis->spi->dev, indio_dev,
+					      &iio_pollfunc_store_time,
+					      trigger_handler, NULL);
+	if (ret)
+		return ret;
+
+	if (adis->spi->irq) {
+		ret = devm_adis_probe_trigger(adis, indio_dev);
+		if (ret)
+			return ret;
+	}
+
+	return devm_add_action_or_reset(&adis->spi->dev, adis_buffer_cleanup,
+					adis);
+}
+EXPORT_SYMBOL_GPL(devm_adis_setup_buffer_and_trigger);
+
+/**
  * adis_cleanup_buffer_and_trigger() - Free buffer and trigger resources
  * @adis: The adis device.
  * @indio_dev: The IIO device.
diff --git a/drivers/iio/imu/adis_trigger.c b/drivers/iio/imu/adis_trigger.c
index 8b9cd02c0f9f..8afe71947c00 100644
--- a/drivers/iio/imu/adis_trigger.c
+++ b/drivers/iio/imu/adis_trigger.c
@@ -27,6 +27,34 @@ static const struct iio_trigger_ops adis_trigger_ops = {
 	.set_trigger_state = &adis_data_rdy_trigger_set_state,
 };
 
+static void adis_trigger_setup(struct adis *adis)
+{
+	adis->trig->dev.parent = &adis->spi->dev;
+	adis->trig->ops = &adis_trigger_ops;
+	iio_trigger_set_drvdata(adis->trig, adis);
+}
+
+static int adis_validate_irq_flag(struct adis *adis)
+{
+	/*
+	 * Typically this devices have data ready either on the rising edge or
+	 * on the falling edge of the data ready pin. This checks enforces that
+	 * one of those is set in the drivers... It defaults to
+	 * IRQF_TRIGGER_RISING for backward compatibility wiht devices that
+	 * don't support changing the pin polarity.
+	 */
+	if (!adis->irq_flag) {
+		adis->irq_flag = IRQF_TRIGGER_RISING;
+		return 0;
+	} else if (adis->irq_flag != IRQF_TRIGGER_RISING &&
+		   adis->irq_flag != IRQF_TRIGGER_FALLING) {
+		dev_err(&adis->spi->dev, "Invalid IRQ mask: %08lx\n",
+			adis->irq_flag);
+		return -EINVAL;
+	}
+
+	return 0;
+}
 /**
  * adis_probe_trigger() - Sets up trigger for a adis device
  * @adis: The adis device
@@ -45,13 +73,15 @@ int adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev)
 	if (adis->trig == NULL)
 		return -ENOMEM;
 
-	adis->trig->dev.parent = &adis->spi->dev;
-	adis->trig->ops = &adis_trigger_ops;
-	iio_trigger_set_drvdata(adis->trig, adis);
+	adis_trigger_setup(adis);
+
+	ret = adis_validate_irq_flag(adis);
+	if (ret)
+		return ret;
 
 	ret = request_irq(adis->spi->irq,
 			  &iio_trigger_generic_data_rdy_poll,
-			  IRQF_TRIGGER_RISING,
+			  adis->irq_flag,
 			  indio_dev->name,
 			  adis->trig);
 	if (ret)
@@ -74,6 +104,40 @@ error_free_trig:
 EXPORT_SYMBOL_GPL(adis_probe_trigger);
 
 /**
+ * devm_adis_probe_trigger() - Sets up trigger for a managed adis device
+ * @adis: The adis device
+ * @indio_dev: The IIO device
+ *
+ * Returns 0 on success or a negative error code
+ */
+int devm_adis_probe_trigger(struct adis *adis, struct iio_dev *indio_dev)
+{
+	int ret;
+
+	adis->trig = devm_iio_trigger_alloc(&adis->spi->dev, "%s-dev%d",
+					    indio_dev->name, indio_dev->id);
+	if (!adis->trig)
+		return -ENOMEM;
+
+	adis_trigger_setup(adis);
+
+	ret = adis_validate_irq_flag(adis);
+	if (ret)
+		return ret;
+
+	ret = devm_request_irq(&adis->spi->dev, adis->spi->irq,
+			       &iio_trigger_generic_data_rdy_poll,
+			       adis->irq_flag,
+			       indio_dev->name,
+			       adis->trig);
+	if (ret)
+		return ret;
+
+	return devm_iio_trigger_register(&adis->spi->dev, adis->trig);
+}
+EXPORT_SYMBOL_GPL(devm_adis_probe_trigger);
+
+/**
  * adis_remove_trigger() - Remove trigger for a adis devices
  * @adis: The adis device
  *
diff --git a/drivers/iio/imu/bmi160/bmi160_i2c.c b/drivers/iio/imu/bmi160/bmi160_i2c.c
index e36f5e82d400..26398614eddf 100644
--- a/drivers/iio/imu/bmi160/bmi160_i2c.c
+++ b/drivers/iio/imu/bmi160/bmi160_i2c.c
@@ -24,8 +24,8 @@ static int bmi160_i2c_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &bmi160_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
diff --git a/drivers/iio/imu/bmi160/bmi160_spi.c b/drivers/iio/imu/bmi160/bmi160_spi.c
index c19e3df35559..61389b41c6d9 100644
--- a/drivers/iio/imu/bmi160/bmi160_spi.c
+++ b/drivers/iio/imu/bmi160/bmi160_spi.c
@@ -20,8 +20,8 @@ static int bmi160_spi_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &bmi160_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 	return bmi160_core_probe(&spi->dev, regmap, id->name, true);
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c
index 2f8560ba4572..c27d06035c8b 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_acpi.c
@@ -135,6 +135,7 @@ int inv_mpu_acpi_create_mux_client(struct i2c_client *client)
 	st->mux_client = NULL;
 	if (ACPI_HANDLE(&client->dev)) {
 		struct i2c_board_info info;
+		struct i2c_client *mux_client;
 		struct acpi_device *adev;
 		int ret = -1;
 
@@ -172,9 +173,10 @@ int inv_mpu_acpi_create_mux_client(struct i2c_client *client)
 			} else
 				return 0; /* no secondary addr, which is OK */
 		}
-		st->mux_client = i2c_new_device(st->muxc->adapter[0], &info);
-		if (!st->mux_client)
-			return -ENODEV;
+		mux_client = i2c_new_client_device(st->muxc->adapter[0], &info);
+		if (IS_ERR(mux_client))
+			return PTR_ERR(mux_client);
+		st->mux_client = mux_client;
 	}
 
 	return 0;
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 0b8d2f7a0165..4d604fe842e5 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -526,7 +526,7 @@ static int inv_mpu6050_sensor_set(struct inv_mpu6050_state  *st, int reg,
 	__be16 d = cpu_to_be16(val);
 
 	ind = (axis - IIO_MOD_X) * 2;
-	result = regmap_bulk_write(st->map, reg + ind, (u8 *)&d, 2);
+	result = regmap_bulk_write(st->map, reg + ind, &d, sizeof(d));
 	if (result)
 		return -EINVAL;
 
@@ -540,7 +540,7 @@ static int inv_mpu6050_sensor_show(struct inv_mpu6050_state  *st, int reg,
 	__be16 d;
 
 	ind = (axis - IIO_MOD_X) * 2;
-	result = regmap_bulk_read(st->map, reg + ind, (u8 *)&d, 2);
+	result = regmap_bulk_read(st->map, reg + ind, &d, sizeof(d));
 	if (result)
 		return -EINVAL;
 	*val = (short)be16_to_cpup(&d);
@@ -1248,12 +1248,31 @@ static const struct attribute_group inv_attribute_group = {
 	.attrs = inv_attributes
 };
 
+static int inv_mpu6050_reg_access(struct iio_dev *indio_dev,
+				  unsigned int reg,
+				  unsigned int writeval,
+				  unsigned int *readval)
+{
+	struct inv_mpu6050_state *st = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&st->lock);
+	if (readval)
+		ret = regmap_read(st->map, reg, readval);
+	else
+		ret = regmap_write(st->map, reg, writeval);
+	mutex_unlock(&st->lock);
+
+	return ret;
+}
+
 static const struct iio_info mpu_info = {
 	.read_raw = &inv_mpu6050_read_raw,
 	.write_raw = &inv_mpu6050_write_raw,
 	.write_raw_get_fmt = &inv_write_raw_get_fmt,
 	.attrs = &inv_attribute_group,
 	.validate_trigger = inv_mpu6050_validate_trigger,
+	.debugfs_reg_access = &inv_mpu6050_reg_access,
 };
 
 /**
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
index 6993d3b87bb0..28cfae1e61cf 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_i2c.c
@@ -122,8 +122,8 @@ static int inv_mpu_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &inv_mpu_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
index 673b198e6368..6f968ce687e1 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_spi.c
@@ -53,8 +53,8 @@ static int inv_mpu_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &inv_mpu_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
index 41cb20cb3809..b56df409ed0f 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx.h
@@ -111,7 +111,7 @@ struct st_lsm6dsx_odr {
 	u8 val;
 };
 
-#define ST_LSM6DSX_ODR_LIST_SIZE	6
+#define ST_LSM6DSX_ODR_LIST_SIZE	8
 struct st_lsm6dsx_odr_table_entry {
 	struct st_lsm6dsx_reg reg;
 
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
index 4426524b59f2..0b776cb91928 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_core.c
@@ -27,7 +27,8 @@
  *   - FIFO size: 4KB
  *
  * - LSM6DSO/LSM6DSOX/ASM330LHH/LSM6DSR/ISM330DHCX:
- *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416
+ *   - Accelerometer/Gyroscope supported ODR [Hz]: 13, 26, 52, 104, 208, 416,
+ *     833
  *   - Accelerometer supported full-scale [g]: +-2/+-4/+-8/+-16
  *   - Gyroscope supported full-scale [dps]: +-125/+-245/+-500/+-1000/+-2000
  *   - FIFO size: 3KB
@@ -791,7 +792,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 			[ST_LSM6DSX_ID_GYRO] = {
 				.reg = {
@@ -804,7 +806,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 		},
 		.fs_table = {
@@ -994,7 +997,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 			[ST_LSM6DSX_ID_GYRO] = {
 				.reg = {
@@ -1007,7 +1011,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 		},
 		.fs_table = {
@@ -1171,7 +1176,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 			[ST_LSM6DSX_ID_GYRO] = {
 				.reg = {
@@ -1184,7 +1190,8 @@ static const struct st_lsm6dsx_settings st_lsm6dsx_sensor_settings[] = {
 				.odr_avl[3] = { 104000, 0x04 },
 				.odr_avl[4] = { 208000, 0x05 },
 				.odr_avl[5] = { 416000, 0x06 },
-				.odr_len = 6,
+				.odr_avl[6] = { 833000, 0x07 },
+				.odr_len = 7,
 			},
 		},
 		.fs_table = {
diff --git a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
index 1cf98195f84d..c1f83fe0d8da 100644
--- a/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
+++ b/drivers/iio/imu/st_lsm6dsx/st_lsm6dsx_shub.c
@@ -88,6 +88,69 @@ static const struct st_lsm6dsx_ext_dev_settings st_lsm6dsx_ext_dev_table[] = {
 			.len = 6,
 		},
 	},
+	/* LIS3MDL */
+	{
+		.i2c_addr = { 0x1e },
+		.wai = {
+			.addr = 0x0f,
+			.val = 0x3d,
+		},
+		.id = ST_LSM6DSX_ID_MAGN,
+		.odr_table = {
+			.reg = {
+				.addr = 0x20,
+				.mask = GENMASK(4, 2),
+			},
+			.odr_avl[0] = {  1000, 0x0 },
+			.odr_avl[1] = {  2000, 0x1 },
+			.odr_avl[2] = {  3000, 0x2 },
+			.odr_avl[3] = {  5000, 0x3 },
+			.odr_avl[4] = { 10000, 0x4 },
+			.odr_avl[5] = { 20000, 0x5 },
+			.odr_avl[6] = { 40000, 0x6 },
+			.odr_avl[7] = { 80000, 0x7 },
+			.odr_len = 8,
+		},
+		.fs_table = {
+			.reg = {
+				.addr = 0x21,
+				.mask = GENMASK(6, 5),
+			},
+			.fs_avl[0] = {
+				.gain = 146,
+				.val = 0x00,
+			}, /* 4000 uG/LSB */
+			.fs_avl[1] = {
+				.gain = 292,
+				.val = 0x01,
+			}, /* 8000 uG/LSB */
+			.fs_avl[2] = {
+				.gain = 438,
+				.val = 0x02,
+			}, /* 12000 uG/LSB */
+			.fs_avl[3] = {
+				.gain = 584,
+				.val = 0x03,
+			}, /* 16000 uG/LSB */
+			.fs_len = 4,
+		},
+		.pwr_table = {
+			.reg = {
+				.addr = 0x22,
+				.mask = GENMASK(1, 0),
+			},
+			.off_val = 0x2,
+			.on_val = 0x0,
+		},
+		.bdu = {
+			.addr = 0x24,
+			.mask = BIT(6),
+		},
+		.out = {
+			.addr = 0x28,
+			.len = 6,
+		},
+	},
 };
 
 static void st_lsm6dsx_shub_wait_complete(struct st_lsm6dsx_hw *hw)
@@ -519,6 +582,36 @@ st_lsm6dsx_shub_read_raw(struct iio_dev *iio_dev,
 }
 
 static int
+st_lsm6dsx_shub_set_full_scale(struct st_lsm6dsx_sensor *sensor,
+			       u32 gain)
+{
+	const struct st_lsm6dsx_fs_table_entry *fs_table;
+	int i, err;
+
+	fs_table = &sensor->ext_info.settings->fs_table;
+	if (!fs_table->reg.addr)
+		return -ENOTSUPP;
+
+	for (i = 0; i < fs_table->fs_len; i++) {
+		if (fs_table->fs_avl[i].gain == gain)
+			break;
+	}
+
+	if (i == fs_table->fs_len)
+		return -EINVAL;
+
+	err = st_lsm6dsx_shub_write_with_mask(sensor, fs_table->reg.addr,
+					      fs_table->reg.mask,
+					      fs_table->fs_avl[i].val);
+	if (err < 0)
+		return err;
+
+	sensor->gain = gain;
+
+	return 0;
+}
+
+static int
 st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
 			  struct iio_chan_spec const *chan,
 			  int val, int val2, long mask)
@@ -554,6 +647,9 @@ st_lsm6dsx_shub_write_raw(struct iio_dev *iio_dev,
 		}
 		break;
 	}
+	case IIO_CHAN_INFO_SCALE:
+		err = st_lsm6dsx_shub_set_full_scale(sensor, val2);
+		break;
 	default:
 		err = -EINVAL;
 		break;
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 4ada5592aa2b..9fa238c0a7d4 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -189,10 +189,12 @@ __poll_t iio_buffer_poll(struct file *filp,
  */
 void iio_buffer_wakeup_poll(struct iio_dev *indio_dev)
 {
-	if (!indio_dev->buffer)
+	struct iio_buffer *buffer = indio_dev->buffer;
+
+	if (!buffer)
 		return;
 
-	wake_up(&indio_dev->buffer->pollq);
+	wake_up(&buffer->pollq);
 }
 
 void iio_buffer_init(struct iio_buffer *buffer)
@@ -262,10 +264,11 @@ static ssize_t iio_scan_el_show(struct device *dev,
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
 
 	/* Ensure ret is 0 or 1. */
 	ret = !!test_bit(to_iio_dev_attr(attr)->address,
-		       indio_dev->buffer->scan_mask);
+		       buffer->scan_mask);
 
 	return sprintf(buf, "%d\n", ret);
 }
@@ -316,8 +319,7 @@ static int iio_scan_mask_set(struct iio_dev *indio_dev,
 	const unsigned long *mask;
 	unsigned long *trialmask;
 
-	trialmask = kcalloc(BITS_TO_LONGS(indio_dev->masklength),
-			    sizeof(*trialmask), GFP_KERNEL);
+	trialmask = bitmap_zalloc(indio_dev->masklength, GFP_KERNEL);
 	if (trialmask == NULL)
 		return -ENOMEM;
 	if (!indio_dev->masklength) {
@@ -382,7 +384,7 @@ static ssize_t iio_scan_el_store(struct device *dev,
 	if (ret < 0)
 		return ret;
 	mutex_lock(&indio_dev->mlock);
-	if (iio_buffer_is_active(indio_dev->buffer)) {
+	if (iio_buffer_is_active(buffer)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
@@ -411,7 +413,9 @@ static ssize_t iio_scan_el_ts_show(struct device *dev,
 				   char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	return sprintf(buf, "%d\n", indio_dev->buffer->scan_timestamp);
+	struct iio_buffer *buffer = indio_dev->buffer;
+
+	return sprintf(buf, "%d\n", buffer->scan_timestamp);
 }
 
 static ssize_t iio_scan_el_ts_store(struct device *dev,
@@ -421,6 +425,7 @@ static ssize_t iio_scan_el_ts_store(struct device *dev,
 {
 	int ret;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
 	bool state;
 
 	ret = strtobool(buf, &state);
@@ -428,11 +433,11 @@ static ssize_t iio_scan_el_ts_store(struct device *dev,
 		return ret;
 
 	mutex_lock(&indio_dev->mlock);
-	if (iio_buffer_is_active(indio_dev->buffer)) {
+	if (iio_buffer_is_active(buffer)) {
 		ret = -EBUSY;
 		goto error_ret;
 	}
-	indio_dev->buffer->scan_timestamp = state;
+	buffer->scan_timestamp = state;
 error_ret:
 	mutex_unlock(&indio_dev->mlock);
 
@@ -440,10 +445,10 @@ error_ret:
 }
 
 static int iio_buffer_add_channel_sysfs(struct iio_dev *indio_dev,
+					struct iio_buffer *buffer,
 					const struct iio_chan_spec *chan)
 {
 	int ret, attrcount = 0;
-	struct iio_buffer *buffer = indio_dev->buffer;
 
 	ret = __iio_add_chan_devattr("index",
 				     chan,
@@ -519,7 +524,7 @@ static ssize_t iio_buffer_write_length(struct device *dev,
 		return len;
 
 	mutex_lock(&indio_dev->mlock);
-	if (iio_buffer_is_active(indio_dev->buffer)) {
+	if (iio_buffer_is_active(buffer)) {
 		ret = -EBUSY;
 	} else {
 		buffer->access->set_length(buffer, val);
@@ -540,7 +545,9 @@ static ssize_t iio_buffer_show_enable(struct device *dev,
 				      char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	return sprintf(buf, "%d\n", iio_buffer_is_active(indio_dev->buffer));
+	struct iio_buffer *buffer = indio_dev->buffer;
+
+	return sprintf(buf, "%d\n", iio_buffer_is_active(buffer));
 }
 
 static unsigned int iio_storage_bytes_for_si(struct iio_dev *indio_dev,
@@ -687,6 +694,13 @@ static int iio_verify_update(struct iio_dev *indio_dev,
 	bool scan_timestamp;
 	unsigned int modes;
 
+	if (insert_buffer &&
+	    bitmap_empty(insert_buffer->scan_mask, indio_dev->masklength)) {
+		dev_dbg(&indio_dev->dev,
+			"At least one scan element must be enabled first\n");
+		return -EINVAL;
+	}
+
 	memset(config, 0, sizeof(*config));
 	config->watermark = ~0;
 
@@ -913,6 +927,7 @@ static int iio_enable_buffers(struct iio_dev *indio_dev,
 	indio_dev->active_scan_mask = config->scan_mask;
 	indio_dev->scan_timestamp = config->scan_timestamp;
 	indio_dev->scan_bytes = config->scan_bytes;
+	indio_dev->currentmode = config->mode;
 
 	iio_update_demux(indio_dev);
 
@@ -948,8 +963,6 @@ static int iio_enable_buffers(struct iio_dev *indio_dev,
 			goto err_disable_buffers;
 	}
 
-	indio_dev->currentmode = config->mode;
-
 	if (indio_dev->setup_ops->postenable) {
 		ret = indio_dev->setup_ops->postenable(indio_dev);
 		if (ret) {
@@ -966,10 +979,10 @@ err_disable_buffers:
 					     buffer_list)
 		iio_buffer_disable(buffer, indio_dev);
 err_run_postdisable:
-	indio_dev->currentmode = INDIO_DIRECT_MODE;
 	if (indio_dev->setup_ops->postdisable)
 		indio_dev->setup_ops->postdisable(indio_dev);
 err_undo_config:
+	indio_dev->currentmode = INDIO_DIRECT_MODE;
 	indio_dev->active_scan_mask = NULL;
 
 	return ret;
@@ -1004,8 +1017,6 @@ static int iio_disable_buffers(struct iio_dev *indio_dev)
 			ret = ret2;
 	}
 
-	indio_dev->currentmode = INDIO_DIRECT_MODE;
-
 	if (indio_dev->setup_ops->postdisable) {
 		ret2 = indio_dev->setup_ops->postdisable(indio_dev);
 		if (ret2 && !ret)
@@ -1014,6 +1025,7 @@ static int iio_disable_buffers(struct iio_dev *indio_dev)
 
 	iio_free_scan_mask(indio_dev, indio_dev->active_scan_mask);
 	indio_dev->active_scan_mask = NULL;
+	indio_dev->currentmode = INDIO_DIRECT_MODE;
 
 	return ret;
 }
@@ -1123,6 +1135,7 @@ static ssize_t iio_buffer_store_enable(struct device *dev,
 	int ret;
 	bool requested_state;
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
+	struct iio_buffer *buffer = indio_dev->buffer;
 	bool inlist;
 
 	ret = strtobool(buf, &requested_state);
@@ -1132,17 +1145,15 @@ static ssize_t iio_buffer_store_enable(struct device *dev,
 	mutex_lock(&indio_dev->mlock);
 
 	/* Find out if it is in the list */
-	inlist = iio_buffer_is_active(indio_dev->buffer);
+	inlist = iio_buffer_is_active(buffer);
 	/* Already in desired state */
 	if (inlist == requested_state)
 		goto done;
 
 	if (requested_state)
-		ret = __iio_update_buffers(indio_dev,
-					 indio_dev->buffer, NULL);
+		ret = __iio_update_buffers(indio_dev, buffer, NULL);
 	else
-		ret = __iio_update_buffers(indio_dev,
-					 NULL, indio_dev->buffer);
+		ret = __iio_update_buffers(indio_dev, NULL, buffer);
 
 done:
 	mutex_unlock(&indio_dev->mlock);
@@ -1184,7 +1195,7 @@ static ssize_t iio_buffer_store_watermark(struct device *dev,
 		goto out;
 	}
 
-	if (iio_buffer_is_active(indio_dev->buffer)) {
+	if (iio_buffer_is_active(buffer)) {
 		ret = -EBUSY;
 		goto out;
 	}
@@ -1201,11 +1212,9 @@ static ssize_t iio_dma_show_data_available(struct device *dev,
 						char *buf)
 {
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
-	size_t bytes;
-
-	bytes = iio_buffer_data_available(indio_dev->buffer);
+	struct iio_buffer *buffer = indio_dev->buffer;
 
-	return sprintf(buf, "%zu\n", bytes);
+	return sprintf(buf, "%zu\n", iio_buffer_data_available(buffer));
 }
 
 static DEVICE_ATTR(length, S_IRUGO | S_IWUSR, iio_buffer_read_length,
@@ -1233,7 +1242,7 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 	struct iio_dev_attr *p;
 	struct attribute **attr;
 	struct iio_buffer *buffer = indio_dev->buffer;
-	int ret, i, attrn, attrcount, attrcount_orig = 0;
+	int ret, i, attrn, attrcount;
 	const struct iio_chan_spec *channels;
 
 	channels = indio_dev->channels;
@@ -1277,12 +1286,7 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 
 	indio_dev->groups[indio_dev->groupcounter++] = &buffer->buffer_group;
 
-	if (buffer->scan_el_attrs != NULL) {
-		attr = buffer->scan_el_attrs->attrs;
-		while (*attr++ != NULL)
-			attrcount_orig++;
-	}
-	attrcount = attrcount_orig;
+	attrcount = 0;
 	INIT_LIST_HEAD(&buffer->scan_el_dev_attr_list);
 	channels = indio_dev->channels;
 	if (channels) {
@@ -1291,7 +1295,7 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 			if (channels[i].scan_index < 0)
 				continue;
 
-			ret = iio_buffer_add_channel_sysfs(indio_dev,
+			ret = iio_buffer_add_channel_sysfs(indio_dev, buffer,
 							 &channels[i]);
 			if (ret < 0)
 				goto error_cleanup_dynamic;
@@ -1319,10 +1323,7 @@ int iio_buffer_alloc_sysfs_and_mask(struct iio_dev *indio_dev)
 		ret = -ENOMEM;
 		goto error_free_scan_mask;
 	}
-	if (buffer->scan_el_attrs)
-		memcpy(buffer->scan_el_group.attrs, buffer->scan_el_attrs,
-		       sizeof(buffer->scan_el_group.attrs[0])*attrcount_orig);
-	attrn = attrcount_orig;
+	attrn = 0;
 
 	list_for_each_entry(p, &buffer->scan_el_dev_attr_list, l)
 		buffer->scan_el_group.attrs[attrn++] = &p->dev_attr.attr;
@@ -1334,20 +1335,22 @@ error_free_scan_mask:
 	bitmap_free(buffer->scan_mask);
 error_cleanup_dynamic:
 	iio_free_chan_devattr_list(&buffer->scan_el_dev_attr_list);
-	kfree(indio_dev->buffer->buffer_group.attrs);
+	kfree(buffer->buffer_group.attrs);
 
 	return ret;
 }
 
 void iio_buffer_free_sysfs_and_mask(struct iio_dev *indio_dev)
 {
-	if (!indio_dev->buffer)
+	struct iio_buffer *buffer = indio_dev->buffer;
+
+	if (!buffer)
 		return;
 
-	bitmap_free(indio_dev->buffer->scan_mask);
-	kfree(indio_dev->buffer->buffer_group.attrs);
-	kfree(indio_dev->buffer->scan_el_group.attrs);
-	iio_free_chan_devattr_list(&indio_dev->buffer->scan_el_dev_attr_list);
+	bitmap_free(buffer->scan_mask);
+	kfree(buffer->buffer_group.attrs);
+	kfree(buffer->scan_el_group.attrs);
+	iio_free_chan_devattr_list(&buffer->scan_el_dev_attr_list);
 }
 
 /**
diff --git a/drivers/iio/industrialio-core.c b/drivers/iio/industrialio-core.c
index 24f7bbff4938..1527f01a44f1 100644
--- a/drivers/iio/industrialio-core.c
+++ b/drivers/iio/industrialio-core.c
@@ -572,46 +572,46 @@ static ssize_t __iio_format_value(char *buf, size_t len, unsigned int type,
 
 	switch (type) {
 	case IIO_VAL_INT:
-		return snprintf(buf, len, "%d", vals[0]);
+		return scnprintf(buf, len, "%d", vals[0]);
 	case IIO_VAL_INT_PLUS_MICRO_DB:
 		scale_db = true;
 		/* fall through */
 	case IIO_VAL_INT_PLUS_MICRO:
 		if (vals[1] < 0)
-			return snprintf(buf, len, "-%d.%06u%s", abs(vals[0]),
+			return scnprintf(buf, len, "-%d.%06u%s", abs(vals[0]),
 					-vals[1], scale_db ? " dB" : "");
 		else
-			return snprintf(buf, len, "%d.%06u%s", vals[0], vals[1],
+			return scnprintf(buf, len, "%d.%06u%s", vals[0], vals[1],
 					scale_db ? " dB" : "");
 	case IIO_VAL_INT_PLUS_NANO:
 		if (vals[1] < 0)
-			return snprintf(buf, len, "-%d.%09u", abs(vals[0]),
+			return scnprintf(buf, len, "-%d.%09u", abs(vals[0]),
 					-vals[1]);
 		else
-			return snprintf(buf, len, "%d.%09u", vals[0], vals[1]);
+			return scnprintf(buf, len, "%d.%09u", vals[0], vals[1]);
 	case IIO_VAL_FRACTIONAL:
 		tmp = div_s64((s64)vals[0] * 1000000000LL, vals[1]);
 		tmp1 = vals[1];
 		tmp0 = (int)div_s64_rem(tmp, 1000000000, &tmp1);
-		return snprintf(buf, len, "%d.%09u", tmp0, abs(tmp1));
+		return scnprintf(buf, len, "%d.%09u", tmp0, abs(tmp1));
 	case IIO_VAL_FRACTIONAL_LOG2:
 		tmp = shift_right((s64)vals[0] * 1000000000LL, vals[1]);
 		tmp0 = (int)div_s64_rem(tmp, 1000000000LL, &tmp1);
-		return snprintf(buf, len, "%d.%09u", tmp0, abs(tmp1));
+		return scnprintf(buf, len, "%d.%09u", tmp0, abs(tmp1));
 	case IIO_VAL_INT_MULTIPLE:
 	{
 		int i;
 		int l = 0;
 
 		for (i = 0; i < size; ++i) {
-			l += snprintf(&buf[l], len - l, "%d ", vals[i]);
+			l += scnprintf(&buf[l], len - l, "%d ", vals[i]);
 			if (l >= len)
 				break;
 		}
 		return l;
 	}
 	case IIO_VAL_CHAR:
-		return snprintf(buf, len, "%c", (char)vals[0]);
+		return scnprintf(buf, len, "%c", (char)vals[0]);
 	default:
 		return 0;
 	}
@@ -682,10 +682,10 @@ static ssize_t iio_format_avail_list(char *buf, const int *vals,
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
 			if (i < length - 1)
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						" ");
 			else
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						"\n");
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
@@ -698,10 +698,10 @@ static ssize_t iio_format_avail_list(char *buf, const int *vals,
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
 			if (i < length / 2 - 1)
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						" ");
 			else
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						"\n");
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
@@ -725,10 +725,10 @@ static ssize_t iio_format_avail_range(char *buf, const int *vals, int type)
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
 			if (i < 2)
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						" ");
 			else
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						"]\n");
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
@@ -741,10 +741,10 @@ static ssize_t iio_format_avail_range(char *buf, const int *vals, int type)
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
 			if (i < 2)
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						" ");
 			else
-				len += snprintf(buf + len, PAGE_SIZE - len,
+				len += scnprintf(buf + len, PAGE_SIZE - len,
 						"]\n");
 			if (len >= PAGE_SIZE)
 				return -EFBIG;
@@ -1507,27 +1507,27 @@ struct iio_dev *iio_device_alloc(int sizeof_priv)
 	alloc_size += IIO_ALIGN - 1;
 
 	dev = kzalloc(alloc_size, GFP_KERNEL);
+	if (!dev)
+		return NULL;
 
-	if (dev) {
-		dev->dev.groups = dev->groups;
-		dev->dev.type = &iio_device_type;
-		dev->dev.bus = &iio_bus_type;
-		device_initialize(&dev->dev);
-		dev_set_drvdata(&dev->dev, (void *)dev);
-		mutex_init(&dev->mlock);
-		mutex_init(&dev->info_exist_lock);
-		INIT_LIST_HEAD(&dev->channel_attr_list);
-
-		dev->id = ida_simple_get(&iio_ida, 0, 0, GFP_KERNEL);
-		if (dev->id < 0) {
-			/* cannot use a dev_err as the name isn't available */
-			pr_err("failed to get device id\n");
-			kfree(dev);
-			return NULL;
-		}
-		dev_set_name(&dev->dev, "iio:device%d", dev->id);
-		INIT_LIST_HEAD(&dev->buffer_list);
+	dev->dev.groups = dev->groups;
+	dev->dev.type = &iio_device_type;
+	dev->dev.bus = &iio_bus_type;
+	device_initialize(&dev->dev);
+	dev_set_drvdata(&dev->dev, (void *)dev);
+	mutex_init(&dev->mlock);
+	mutex_init(&dev->info_exist_lock);
+	INIT_LIST_HEAD(&dev->channel_attr_list);
+
+	dev->id = ida_simple_get(&iio_ida, 0, 0, GFP_KERNEL);
+	if (dev->id < 0) {
+		/* cannot use a dev_err as the name isn't available */
+		pr_err("failed to get device id\n");
+		kfree(dev);
+		return NULL;
 	}
+	dev_set_name(&dev->dev, "iio:device%d", dev->id);
+	INIT_LIST_HEAD(&dev->buffer_list);
 
 	return dev;
 }
@@ -1549,17 +1549,6 @@ static void devm_iio_device_release(struct device *dev, void *res)
 	iio_device_free(*(struct iio_dev **)res);
 }
 
-int devm_iio_device_match(struct device *dev, void *res, void *data)
-{
-	struct iio_dev **r = res;
-	if (!r || !*r) {
-		WARN_ON(!r || !*r);
-		return 0;
-	}
-	return *r == data;
-}
-EXPORT_SYMBOL_GPL(devm_iio_device_match);
-
 /**
  * devm_iio_device_alloc - Resource-managed iio_device_alloc()
  * @dev:		Device to allocate iio_dev for
@@ -1568,9 +1557,6 @@ EXPORT_SYMBOL_GPL(devm_iio_device_match);
  * Managed iio_device_alloc. iio_dev allocated with this function is
  * automatically freed on driver detach.
  *
- * If an iio_dev allocated with this function needs to be freed separately,
- * devm_iio_device_free() must be used.
- *
  * RETURNS:
  * Pointer to allocated iio_dev on success, NULL on failure.
  */
@@ -1596,23 +1582,6 @@ struct iio_dev *devm_iio_device_alloc(struct device *dev, int sizeof_priv)
 EXPORT_SYMBOL_GPL(devm_iio_device_alloc);
 
 /**
- * devm_iio_device_free - Resource-managed iio_device_free()
- * @dev:		Device this iio_dev belongs to
- * @iio_dev:		the iio_dev associated with the device
- *
- * Free iio_dev allocated with devm_iio_device_alloc().
- */
-void devm_iio_device_free(struct device *dev, struct iio_dev *iio_dev)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_device_release,
-			    devm_iio_device_match, iio_dev);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_device_free);
-
-/**
  * iio_chrdev_open() - chrdev file open for buffer access and ioctls
  * @inode:	Inode structure for identifying the device in the file system
  * @filp:	File structure for iio device used to keep and later access
@@ -1714,6 +1683,9 @@ int __iio_device_register(struct iio_dev *indio_dev, struct module *this_mod)
 {
 	int ret;
 
+	if (!indio_dev->info)
+		return -EINVAL;
+
 	indio_dev->driver_module = this_mod;
 	/* If the calling driver did not initialize of_node, do it here */
 	if (!indio_dev->dev.of_node && indio_dev->dev.parent)
@@ -1726,9 +1698,6 @@ int __iio_device_register(struct iio_dev *indio_dev, struct module *this_mod)
 	if (ret < 0)
 		return ret;
 
-	if (!indio_dev->info)
-		return -EINVAL;
-
 	/* configure elements for the chrdev */
 	indio_dev->dev.devt = MKDEV(MAJOR(iio_devt), indio_dev->id);
 
@@ -1834,23 +1803,6 @@ int __devm_iio_device_register(struct device *dev, struct iio_dev *indio_dev,
 EXPORT_SYMBOL_GPL(__devm_iio_device_register);
 
 /**
- * devm_iio_device_unregister - Resource-managed iio_device_unregister()
- * @dev:	Device this iio_dev belongs to
- * @indio_dev:	the iio_dev associated with the device
- *
- * Unregister iio_dev registered with devm_iio_device_register().
- */
-void devm_iio_device_unregister(struct device *dev, struct iio_dev *indio_dev)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_device_unreg,
-			    devm_iio_device_match, indio_dev);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_device_unregister);
-
-/**
  * iio_device_claim_direct_mode - Keep device in direct mode
  * @indio_dev:	the iio_dev associated with the device
  *
diff --git a/drivers/iio/industrialio-trigger.c b/drivers/iio/industrialio-trigger.c
index 3908a9a90035..53d1931f6be8 100644
--- a/drivers/iio/industrialio-trigger.c
+++ b/drivers/iio/industrialio-trigger.c
@@ -585,18 +585,6 @@ static void devm_iio_trigger_release(struct device *dev, void *res)
 	iio_trigger_free(*(struct iio_trigger **)res);
 }
 
-static int devm_iio_trigger_match(struct device *dev, void *res, void *data)
-{
-	struct iio_trigger **r = res;
-
-	if (!r || !*r) {
-		WARN_ON(!r || !*r);
-		return 0;
-	}
-
-	return *r == data;
-}
-
 /**
  * devm_iio_trigger_alloc - Resource-managed iio_trigger_alloc()
  * @dev:		Device to allocate iio_trigger for
@@ -608,9 +596,6 @@ static int devm_iio_trigger_match(struct device *dev, void *res, void *data)
  * Managed iio_trigger_alloc.  iio_trigger allocated with this function is
  * automatically freed on driver detach.
  *
- * If an iio_trigger allocated with this function needs to be freed separately,
- * devm_iio_trigger_free() must be used.
- *
  * RETURNS:
  * Pointer to allocated iio_trigger on success, NULL on failure.
  */
@@ -640,23 +625,6 @@ struct iio_trigger *devm_iio_trigger_alloc(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(devm_iio_trigger_alloc);
 
-/**
- * devm_iio_trigger_free - Resource-managed iio_trigger_free()
- * @dev:		Device this iio_dev belongs to
- * @iio_trig:		the iio_trigger associated with the device
- *
- * Free iio_trigger allocated with devm_iio_trigger_alloc().
- */
-void devm_iio_trigger_free(struct device *dev, struct iio_trigger *iio_trig)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_trigger_release,
-			    devm_iio_trigger_match, iio_trig);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_trigger_free);
-
 static void devm_iio_trigger_unreg(struct device *dev, void *res)
 {
 	iio_trigger_unregister(*(struct iio_trigger **)res);
@@ -673,9 +641,6 @@ static void devm_iio_trigger_unreg(struct device *dev, void *res)
  * calls iio_trigger_register() internally. Refer to that function for more
  * information.
  *
- * If an iio_trigger registered with this function needs to be unregistered
- * separately, devm_iio_trigger_unregister() must be used.
- *
  * RETURNS:
  * 0 on success, negative error number on failure.
  */
@@ -701,24 +666,6 @@ int __devm_iio_trigger_register(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(__devm_iio_trigger_register);
 
-/**
- * devm_iio_trigger_unregister - Resource-managed iio_trigger_unregister()
- * @dev:	device this iio_trigger belongs to
- * @trig_info:	the trigger associated with the device
- *
- * Unregister trigger registered with devm_iio_trigger_register().
- */
-void devm_iio_trigger_unregister(struct device *dev,
-				 struct iio_trigger *trig_info)
-{
-	int rc;
-
-	rc = devres_release(dev, devm_iio_trigger_unreg, devm_iio_trigger_match,
-			    trig_info);
-	WARN_ON(rc);
-}
-EXPORT_SYMBOL_GPL(devm_iio_trigger_unregister);
-
 bool iio_trigger_using_own(struct iio_dev *indio_dev)
 {
 	return indio_dev->trig->attached_own_device;
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index 5a8351c9a426..ede99e0d5371 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -360,18 +360,6 @@ static void devm_iio_channel_free(struct device *dev, void *res)
 	iio_channel_release(channel);
 }
 
-static int devm_iio_channel_match(struct device *dev, void *res, void *data)
-{
-	struct iio_channel **r = res;
-
-	if (!r || !*r) {
-		WARN_ON(!r || !*r);
-		return 0;
-	}
-
-	return *r == data;
-}
-
 struct iio_channel *devm_iio_channel_get(struct device *dev,
 					 const char *channel_name)
 {
@@ -394,13 +382,6 @@ struct iio_channel *devm_iio_channel_get(struct device *dev,
 }
 EXPORT_SYMBOL_GPL(devm_iio_channel_get);
 
-void devm_iio_channel_release(struct device *dev, struct iio_channel *channel)
-{
-	WARN_ON(devres_release(dev, devm_iio_channel_free,
-			       devm_iio_channel_match, channel));
-}
-EXPORT_SYMBOL_GPL(devm_iio_channel_release);
-
 struct iio_channel *iio_channel_get_all(struct device *dev)
 {
 	const char *name;
@@ -514,14 +495,6 @@ struct iio_channel *devm_iio_channel_get_all(struct device *dev)
 }
 EXPORT_SYMBOL_GPL(devm_iio_channel_get_all);
 
-void devm_iio_channel_release_all(struct device *dev,
-				  struct iio_channel *channels)
-{
-	WARN_ON(devres_release(dev, devm_iio_channel_free_all,
-			       devm_iio_channel_match, channels));
-}
-EXPORT_SYMBOL_GPL(devm_iio_channel_release_all);
-
 static int iio_channel_read(struct iio_channel *chan, int *val, int *val2,
 	enum iio_chan_info_enum info)
 {
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index b27719cefcf9..182bd18c4bb2 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -516,6 +516,8 @@ config US5182D
 
 config VCNL4000
 	tristate "VCNL4000/4010/4020/4200 combined ALS and proximity sensor"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
 	depends on I2C
 	help
 	  Say Y here if you want to build a driver for the Vishay VCNL4000,
diff --git a/drivers/iio/light/bh1780.c b/drivers/iio/light/bh1780.c
index a8361006dcd9..03f2d8d123c4 100644
--- a/drivers/iio/light/bh1780.c
+++ b/drivers/iio/light/bh1780.c
@@ -13,7 +13,7 @@
 #include <linux/platform_device.h>
 #include <linux/delay.h>
 #include <linux/module.h>
-#include <linux/of.h>
+#include <linux/mod_devicetable.h>
 #include <linux/pm_runtime.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
@@ -273,13 +273,11 @@ static const struct i2c_device_id bh1780_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, bh1780_id);
 
-#ifdef CONFIG_OF
 static const struct of_device_id of_bh1780_match[] = {
 	{ .compatible = "rohm,bh1780gli", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, of_bh1780_match);
-#endif
 
 static struct i2c_driver bh1780_driver = {
 	.probe		= bh1780_probe,
@@ -288,7 +286,7 @@ static struct i2c_driver bh1780_driver = {
 	.driver = {
 		.name = "bh1780",
 		.pm = &bh1780_dev_pm_ops,
-		.of_match_table = of_match_ptr(of_bh1780_match),
+		.of_match_table = of_bh1780_match,
 	},
 };
 
diff --git a/drivers/iio/light/cm32181.c b/drivers/iio/light/cm32181.c
index 5f4fb5674fa0..160eb3f99795 100644
--- a/drivers/iio/light/cm32181.c
+++ b/drivers/iio/light/cm32181.c
@@ -4,11 +4,13 @@
  * Author: Kevin Tsai <ktsai@capellamicro.com>
  */
 
+#include <linux/acpi.h>
 #include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/interrupt.h>
 #include <linux/regulator/consumer.h>
 #include <linux/iio/iio.h>
@@ -18,17 +20,24 @@
 
 /* Registers Address */
 #define CM32181_REG_ADDR_CMD		0x00
+#define CM32181_REG_ADDR_WH		0x01
+#define CM32181_REG_ADDR_WL		0x02
+#define CM32181_REG_ADDR_TEST		0x03
 #define CM32181_REG_ADDR_ALS		0x04
 #define CM32181_REG_ADDR_STATUS		0x06
 #define CM32181_REG_ADDR_ID		0x07
 
 /* Number of Configurable Registers */
-#define CM32181_CONF_REG_NUM		0x01
+#define CM32181_CONF_REG_NUM		4
 
 /* CMD register */
-#define CM32181_CMD_ALS_ENABLE		0x00
-#define CM32181_CMD_ALS_DISABLE		0x01
-#define CM32181_CMD_ALS_INT_EN		0x02
+#define CM32181_CMD_ALS_DISABLE		BIT(0)
+#define CM32181_CMD_ALS_INT_EN		BIT(1)
+#define CM32181_CMD_ALS_THRES_WINDOW	BIT(2)
+
+#define CM32181_CMD_ALS_PERS_SHIFT	4
+#define CM32181_CMD_ALS_PERS_MASK	(0x03 << CM32181_CMD_ALS_PERS_SHIFT)
+#define CM32181_CMD_ALS_PERS_DEFAULT	(0x01 << CM32181_CMD_ALS_PERS_SHIFT)
 
 #define CM32181_CMD_ALS_IT_SHIFT	6
 #define CM32181_CMD_ALS_IT_MASK		(0x0F << CM32181_CMD_ALS_IT_SHIFT)
@@ -38,27 +47,133 @@
 #define CM32181_CMD_ALS_SM_MASK		(0x03 << CM32181_CMD_ALS_SM_SHIFT)
 #define CM32181_CMD_ALS_SM_DEFAULT	(0x01 << CM32181_CMD_ALS_SM_SHIFT)
 
-#define CM32181_MLUX_PER_BIT		5	/* ALS_SM=01 IT=800ms */
-#define CM32181_MLUX_PER_BIT_BASE_IT	800000	/* Based on IT=800ms */
-#define	CM32181_CALIBSCALE_DEFAULT	1000
-#define CM32181_CALIBSCALE_RESOLUTION	1000
-#define MLUX_PER_LUX			1000
+#define CM32181_LUX_PER_BIT		500	/* ALS_SM=01 IT=800ms */
+#define CM32181_LUX_PER_BIT_RESOLUTION	100000
+#define CM32181_LUX_PER_BIT_BASE_IT	800000	/* Based on IT=800ms */
+#define CM32181_CALIBSCALE_DEFAULT	100000
+#define CM32181_CALIBSCALE_RESOLUTION	100000
 
-static const u8 cm32181_reg[CM32181_CONF_REG_NUM] = {
-	CM32181_REG_ADDR_CMD,
-};
+#define SMBUS_ALERT_RESPONSE_ADDRESS	0x0c
+
+/* CPM0 Index 0: device-id (3218 or 32181), 1: Unknown, 2: init_regs_bitmap */
+#define CPM0_REGS_BITMAP		2
+#define CPM0_HEADER_SIZE		3
 
-static const int als_it_bits[] = {12, 8, 0, 1, 2, 3};
-static const int als_it_value[] = {25000, 50000, 100000, 200000, 400000,
-	800000};
+/* CPM1 Index 0: lux_per_bit, 1: calibscale, 2: resolution (100000) */
+#define CPM1_LUX_PER_BIT		0
+#define CPM1_CALIBSCALE			1
+#define CPM1_SIZE			3
+
+/* CM3218 Family */
+static const int cm3218_als_it_bits[] = { 0, 1, 2, 3 };
+static const int cm3218_als_it_values[] = { 100000, 200000, 400000, 800000 };
+
+/* CM32181 Family */
+static const int cm32181_als_it_bits[] = { 12, 8, 0, 1, 2, 3 };
+static const int cm32181_als_it_values[] = {
+	25000, 50000, 100000, 200000, 400000, 800000
+};
 
 struct cm32181_chip {
 	struct i2c_client *client;
+	struct device *dev;
 	struct mutex lock;
 	u16 conf_regs[CM32181_CONF_REG_NUM];
+	unsigned long init_regs_bitmap;
 	int calibscale;
+	int lux_per_bit;
+	int lux_per_bit_base_it;
+	int num_als_it;
+	const int *als_it_bits;
+	const int *als_it_values;
 };
 
+static int cm32181_read_als_it(struct cm32181_chip *cm32181, int *val2);
+
+#ifdef CONFIG_ACPI
+/**
+ * cm32181_acpi_get_cpm() - Get CPM object from ACPI
+ * @client	pointer of struct i2c_client.
+ * @obj_name	pointer of ACPI object name.
+ * @count	maximum size of return array.
+ * @vals	pointer of array for return elements.
+ *
+ * Convert ACPI CPM table to array.
+ *
+ * Return: -ENODEV for fail.  Otherwise is number of elements.
+ */
+static int cm32181_acpi_get_cpm(struct device *dev, char *obj_name,
+				u64 *values, int count)
+{
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *cpm, *elem;
+	acpi_handle handle;
+	acpi_status status;
+	int i;
+
+	handle = ACPI_HANDLE(dev);
+	if (!handle)
+		return -ENODEV;
+
+	status = acpi_evaluate_object(handle, obj_name, NULL, &buffer);
+	if (ACPI_FAILURE(status)) {
+		dev_err(dev, "object %s not found\n", obj_name);
+		return -ENODEV;
+	}
+
+	cpm = buffer.pointer;
+	if (cpm->package.count > count)
+		dev_warn(dev, "%s table contains %u values, only using first %d values\n",
+			 obj_name, cpm->package.count, count);
+
+	count = min_t(int, cpm->package.count, count);
+	for (i = 0; i < count; i++) {
+		elem = &(cpm->package.elements[i]);
+		values[i] = elem->integer.value;
+	}
+
+	kfree(buffer.pointer);
+
+	return count;
+}
+
+static void cm32181_acpi_parse_cpm_tables(struct cm32181_chip *cm32181)
+{
+	u64 vals[CPM0_HEADER_SIZE + CM32181_CONF_REG_NUM];
+	struct device *dev = cm32181->dev;
+	int i, count;
+
+	count = cm32181_acpi_get_cpm(dev, "CPM0", vals, ARRAY_SIZE(vals));
+	if (count <= CPM0_HEADER_SIZE)
+		return;
+
+	count -= CPM0_HEADER_SIZE;
+
+	cm32181->init_regs_bitmap = vals[CPM0_REGS_BITMAP];
+	cm32181->init_regs_bitmap &= GENMASK(count - 1, 0);
+	for_each_set_bit(i, &cm32181->init_regs_bitmap, count)
+		cm32181->conf_regs[i] =	vals[CPM0_HEADER_SIZE + i];
+
+	count = cm32181_acpi_get_cpm(dev, "CPM1", vals, ARRAY_SIZE(vals));
+	if (count != CPM1_SIZE)
+		return;
+
+	cm32181->lux_per_bit = vals[CPM1_LUX_PER_BIT];
+
+	/* Check for uncalibrated devices */
+	if (vals[CPM1_CALIBSCALE] == CM32181_CALIBSCALE_DEFAULT)
+		return;
+
+	cm32181->calibscale = vals[CPM1_CALIBSCALE];
+	/* CPM1 lux_per_bit is for the current it value */
+	cm32181_read_als_it(cm32181, &cm32181->lux_per_bit_base_it);
+}
+#else
+static void cm32181_acpi_parse_cpm_tables(struct cm32181_chip *cm32181)
+{
+}
+#endif /* CONFIG_ACPI */
+
 /**
  * cm32181_reg_init() - Initialize CM32181 registers
  * @cm32181:	pointer of struct cm32181.
@@ -78,18 +193,37 @@ static int cm32181_reg_init(struct cm32181_chip *cm32181)
 		return ret;
 
 	/* check device ID */
-	if ((ret & 0xFF) != 0x81)
+	switch (ret & 0xFF) {
+	case 0x18: /* CM3218 */
+		cm32181->num_als_it = ARRAY_SIZE(cm3218_als_it_bits);
+		cm32181->als_it_bits = cm3218_als_it_bits;
+		cm32181->als_it_values = cm3218_als_it_values;
+		break;
+	case 0x81: /* CM32181 */
+	case 0x82: /* CM32182, fully compat. with CM32181 */
+		cm32181->num_als_it = ARRAY_SIZE(cm32181_als_it_bits);
+		cm32181->als_it_bits = cm32181_als_it_bits;
+		cm32181->als_it_values = cm32181_als_it_values;
+		break;
+	default:
 		return -ENODEV;
+	}
 
 	/* Default Values */
-	cm32181->conf_regs[CM32181_REG_ADDR_CMD] = CM32181_CMD_ALS_ENABLE |
+	cm32181->conf_regs[CM32181_REG_ADDR_CMD] =
 			CM32181_CMD_ALS_IT_DEFAULT | CM32181_CMD_ALS_SM_DEFAULT;
+	cm32181->init_regs_bitmap = BIT(CM32181_REG_ADDR_CMD);
 	cm32181->calibscale = CM32181_CALIBSCALE_DEFAULT;
+	cm32181->lux_per_bit = CM32181_LUX_PER_BIT;
+	cm32181->lux_per_bit_base_it = CM32181_LUX_PER_BIT_BASE_IT;
+
+	if (ACPI_HANDLE(cm32181->dev))
+		cm32181_acpi_parse_cpm_tables(cm32181);
 
 	/* Initialize registers*/
-	for (i = 0; i < CM32181_CONF_REG_NUM; i++) {
-		ret = i2c_smbus_write_word_data(client, cm32181_reg[i],
-			cm32181->conf_regs[i]);
+	for_each_set_bit(i, &cm32181->init_regs_bitmap, CM32181_CONF_REG_NUM) {
+		ret = i2c_smbus_write_word_data(client, i,
+						cm32181->conf_regs[i]);
 		if (ret < 0)
 			return ret;
 	}
@@ -102,7 +236,7 @@ static int cm32181_reg_init(struct cm32181_chip *cm32181)
  *  @cm32181:	pointer of struct cm32181
  *  @val2:	pointer of int to load the als_it value.
  *
- *  Report the current integartion time by millisecond.
+ *  Report the current integration time in milliseconds.
  *
  *  Return: IIO_VAL_INT_PLUS_MICRO for success, otherwise -EINVAL.
  */
@@ -114,9 +248,9 @@ static int cm32181_read_als_it(struct cm32181_chip *cm32181, int *val2)
 	als_it = cm32181->conf_regs[CM32181_REG_ADDR_CMD];
 	als_it &= CM32181_CMD_ALS_IT_MASK;
 	als_it >>= CM32181_CMD_ALS_IT_SHIFT;
-	for (i = 0; i < ARRAY_SIZE(als_it_bits); i++) {
-		if (als_it == als_it_bits[i]) {
-			*val2 = als_it_value[i];
+	for (i = 0; i < cm32181->num_als_it; i++) {
+		if (als_it == cm32181->als_it_bits[i]) {
+			*val2 = cm32181->als_it_values[i];
 			return IIO_VAL_INT_PLUS_MICRO;
 		}
 	}
@@ -139,14 +273,14 @@ static int cm32181_write_als_it(struct cm32181_chip *cm32181, int val)
 	u16 als_it;
 	int ret, i, n;
 
-	n = ARRAY_SIZE(als_it_value);
+	n = cm32181->num_als_it;
 	for (i = 0; i < n; i++)
-		if (val <= als_it_value[i])
+		if (val <= cm32181->als_it_values[i])
 			break;
 	if (i >= n)
 		i = n - 1;
 
-	als_it = als_it_bits[i];
+	als_it = cm32181->als_it_bits[i];
 	als_it <<= CM32181_CMD_ALS_IT_SHIFT;
 
 	mutex_lock(&cm32181->lock);
@@ -175,15 +309,15 @@ static int cm32181_get_lux(struct cm32181_chip *cm32181)
 	struct i2c_client *client = cm32181->client;
 	int ret;
 	int als_it;
-	unsigned long lux;
+	u64 lux;
 
 	ret = cm32181_read_als_it(cm32181, &als_it);
 	if (ret < 0)
 		return -EINVAL;
 
-	lux = CM32181_MLUX_PER_BIT;
-	lux *= CM32181_MLUX_PER_BIT_BASE_IT;
-	lux /= als_it;
+	lux = cm32181->lux_per_bit;
+	lux *= cm32181->lux_per_bit_base_it;
+	lux = div_u64(lux, als_it);
 
 	ret = i2c_smbus_read_word_data(client, CM32181_REG_ADDR_ALS);
 	if (ret < 0)
@@ -191,8 +325,8 @@ static int cm32181_get_lux(struct cm32181_chip *cm32181)
 
 	lux *= ret;
 	lux *= cm32181->calibscale;
-	lux /= CM32181_CALIBSCALE_RESOLUTION;
-	lux /= MLUX_PER_LUX;
+	lux = div_u64(lux, CM32181_CALIBSCALE_RESOLUTION);
+	lux = div_u64(lux, CM32181_LUX_PER_BIT_RESOLUTION);
 
 	if (lux > 0xFFFF)
 		lux = 0xFFFF;
@@ -258,11 +392,12 @@ static int cm32181_write_raw(struct iio_dev *indio_dev,
 static ssize_t cm32181_get_it_available(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
+	struct cm32181_chip *cm32181 = iio_priv(dev_to_iio_dev(dev));
 	int i, n, len;
 
-	n = ARRAY_SIZE(als_it_value);
+	n = cm32181->num_als_it;
 	for (i = 0, len = 0; i < n; i++)
-		len += sprintf(buf + len, "0.%06u ", als_it_value[i]);
+		len += sprintf(buf + len, "0.%06u ", cm32181->als_it_values[i]);
 	return len + sprintf(buf + len, "\n");
 }
 
@@ -294,70 +429,86 @@ static const struct iio_info cm32181_info = {
 	.attrs			= &cm32181_attribute_group,
 };
 
-static int cm32181_probe(struct i2c_client *client,
-			const struct i2c_device_id *id)
+static int cm32181_probe(struct i2c_client *client)
 {
+	struct device *dev = &client->dev;
 	struct cm32181_chip *cm32181;
 	struct iio_dev *indio_dev;
 	int ret;
 
-	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*cm32181));
-	if (!indio_dev) {
-		dev_err(&client->dev, "devm_iio_device_alloc failed\n");
+	indio_dev = devm_iio_device_alloc(dev, sizeof(*cm32181));
+	if (!indio_dev)
 		return -ENOMEM;
+
+	/*
+	 * Some ACPI systems list 2 I2C resources for the CM3218 sensor, the
+	 * SMBus Alert Response Address (ARA, 0x0c) and the actual I2C address.
+	 * Detect this and take the following step to deal with it:
+	 * 1. When a SMBus Alert capable sensor has an Alert asserted, it will
+	 *    not respond on its actual I2C address. Read a byte from the ARA
+	 *    to clear any pending Alerts.
+	 * 2. Create a "dummy" client for the actual I2C address and
+	 *    use that client to communicate with the sensor.
+	 */
+	if (ACPI_HANDLE(dev) && client->addr == SMBUS_ALERT_RESPONSE_ADDRESS) {
+		struct i2c_board_info board_info = { .type = "dummy" };
+
+		i2c_smbus_read_byte(client);
+
+		client = i2c_acpi_new_device(dev, 1, &board_info);
+		if (IS_ERR(client))
+			return PTR_ERR(client);
 	}
 
 	cm32181 = iio_priv(indio_dev);
-	i2c_set_clientdata(client, indio_dev);
 	cm32181->client = client;
+	cm32181->dev = dev;
 
 	mutex_init(&cm32181->lock);
-	indio_dev->dev.parent = &client->dev;
+	indio_dev->dev.parent = dev;
 	indio_dev->channels = cm32181_channels;
 	indio_dev->num_channels = ARRAY_SIZE(cm32181_channels);
 	indio_dev->info = &cm32181_info;
-	indio_dev->name = id->name;
+	indio_dev->name = dev_name(dev);
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
 	ret = cm32181_reg_init(cm32181);
 	if (ret) {
-		dev_err(&client->dev,
-			"%s: register init failed\n",
-			__func__);
+		dev_err(dev, "%s: register init failed\n", __func__);
 		return ret;
 	}
 
-	ret = devm_iio_device_register(&client->dev, indio_dev);
+	ret = devm_iio_device_register(dev, indio_dev);
 	if (ret) {
-		dev_err(&client->dev,
-			"%s: regist device failed\n",
-			__func__);
+		dev_err(dev, "%s: regist device failed\n", __func__);
 		return ret;
 	}
 
 	return 0;
 }
 
-static const struct i2c_device_id cm32181_id[] = {
-	{ "cm32181", 0 },
-	{ }
-};
-
-MODULE_DEVICE_TABLE(i2c, cm32181_id);
-
 static const struct of_device_id cm32181_of_match[] = {
+	{ .compatible = "capella,cm3218" },
 	{ .compatible = "capella,cm32181" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, cm32181_of_match);
 
+#ifdef CONFIG_ACPI
+static const struct acpi_device_id cm32181_acpi_match[] = {
+	{ "CPLM3218", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(acpi, cm32181_acpi_match);
+#endif
+
 static struct i2c_driver cm32181_driver = {
 	.driver = {
 		.name	= "cm32181",
-		.of_match_table = of_match_ptr(cm32181_of_match),
+		.acpi_match_table = ACPI_PTR(cm32181_acpi_match),
+		.of_match_table = cm32181_of_match,
 	},
-	.id_table       = cm32181_id,
-	.probe		= cm32181_probe,
+	.probe_new	= cm32181_probe,
 };
 
 module_i2c_driver(cm32181_driver);
diff --git a/drivers/iio/light/cm3232.c b/drivers/iio/light/cm3232.c
index cd3cfb7d02bd..867200825686 100644
--- a/drivers/iio/light/cm3232.c
+++ b/drivers/iio/light/cm3232.c
@@ -10,6 +10,7 @@
 
 #include <linux/i2c.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/init.h>
@@ -418,7 +419,7 @@ MODULE_DEVICE_TABLE(of, cm3232_of_match);
 static struct i2c_driver cm3232_driver = {
 	.driver = {
 		.name	= "cm3232",
-		.of_match_table = of_match_ptr(cm3232_of_match),
+		.of_match_table = cm3232_of_match,
 #ifdef CONFIG_PM_SLEEP
 		.pm	= &cm3232_pm_ops,
 #endif
diff --git a/drivers/iio/light/gp2ap002.c b/drivers/iio/light/gp2ap002.c
index b7ef16b28280..7a2679bdc987 100644
--- a/drivers/iio/light/gp2ap002.c
+++ b/drivers/iio/light/gp2ap002.c
@@ -158,6 +158,9 @@ static irqreturn_t gp2ap002_prox_irq(int irq, void *d)
 	int val;
 	int ret;
 
+	if (!gp2ap002->enabled)
+		goto err_retrig;
+
 	ret = regmap_read(gp2ap002->map, GP2AP002_PROX, &val);
 	if (ret) {
 		dev_err(gp2ap002->dev, "error reading proximity\n");
@@ -247,6 +250,8 @@ static int gp2ap002_read_raw(struct iio_dev *indio_dev,
 	struct gp2ap002 *gp2ap002 = iio_priv(indio_dev);
 	int ret;
 
+	pm_runtime_get_sync(gp2ap002->dev);
+
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		switch (chan->type) {
@@ -255,13 +260,21 @@ static int gp2ap002_read_raw(struct iio_dev *indio_dev,
 			if (ret < 0)
 				return ret;
 			*val = ret;
-			return IIO_VAL_INT;
+			ret = IIO_VAL_INT;
+			goto out;
 		default:
-			return -EINVAL;
+			ret = -EINVAL;
+			goto out;
 		}
 	default:
-		return -EINVAL;
+		ret = -EINVAL;
 	}
+
+out:
+	pm_runtime_mark_last_busy(gp2ap002->dev);
+	pm_runtime_put_autosuspend(gp2ap002->dev);
+
+	return ret;
 }
 
 static int gp2ap002_init(struct gp2ap002 *gp2ap002)
diff --git a/drivers/iio/light/gp2ap020a00f.c b/drivers/iio/light/gp2ap020a00f.c
index 7fbbce0d4bc7..070d4cd0cf54 100644
--- a/drivers/iio/light/gp2ap020a00f.c
+++ b/drivers/iio/light/gp2ap020a00f.c
@@ -38,8 +38,8 @@
 #include <linux/irq.h>
 #include <linux/irq_work.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/mutex.h>
-#include <linux/of.h>
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
 #include <linux/slab.h>
@@ -1617,18 +1617,16 @@ static const struct i2c_device_id gp2ap020a00f_id[] = {
 
 MODULE_DEVICE_TABLE(i2c, gp2ap020a00f_id);
 
-#ifdef CONFIG_OF
 static const struct of_device_id gp2ap020a00f_of_match[] = {
 	{ .compatible = "sharp,gp2ap020a00f" },
 	{ }
 };
 MODULE_DEVICE_TABLE(of, gp2ap020a00f_of_match);
-#endif
 
 static struct i2c_driver gp2ap020a00f_driver = {
 	.driver = {
 		.name	= GP2A_I2C_NAME,
-		.of_match_table = of_match_ptr(gp2ap020a00f_of_match),
+		.of_match_table = gp2ap020a00f_of_match,
 	},
 	.probe		= gp2ap020a00f_probe,
 	.remove		= gp2ap020a00f_remove,
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index b6cd299517d1..81fa2a422797 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 enum {
@@ -308,18 +306,13 @@ static int hid_als_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&als_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&als_state->common_attributes);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -343,9 +336,7 @@ static int hid_als_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&als_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &als_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -360,8 +351,7 @@ static int hid_als_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_ALS);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&als_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &als_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 7e1030af9ba3..e9c04df07344 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 #define CHANNEL_SCAN_INDEX_PRESENCE 0
@@ -286,18 +284,13 @@ static int hid_prox_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&prox_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&prox_state->common_attributes);
 	if (ret) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -321,9 +314,7 @@ static int hid_prox_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&prox_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &prox_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -338,8 +329,7 @@ static int hid_prox_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_PROX);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&prox_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &prox_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/light/isl29125.c b/drivers/iio/light/isl29125.c
index e37894f0ae0b..95611f5eff01 100644
--- a/drivers/iio/light/isl29125.c
+++ b/drivers/iio/light/isl29125.c
@@ -213,13 +213,24 @@ static const struct iio_info isl29125_info = {
 	.attrs = &isl29125_attribute_group,
 };
 
-static int isl29125_buffer_preenable(struct iio_dev *indio_dev)
+static int isl29125_buffer_postenable(struct iio_dev *indio_dev)
 {
 	struct isl29125_data *data = iio_priv(indio_dev);
+	int err;
+
+	err = iio_triggered_buffer_postenable(indio_dev);
+	if (err)
+		return err;
 
 	data->conf1 |= ISL29125_MODE_RGB;
-	return i2c_smbus_write_byte_data(data->client, ISL29125_CONF1,
+	err = i2c_smbus_write_byte_data(data->client, ISL29125_CONF1,
 		data->conf1);
+	if (err) {
+		iio_triggered_buffer_predisable(indio_dev);
+		return err;
+	}
+
+	return 0;
 }
 
 static int isl29125_buffer_predisable(struct iio_dev *indio_dev)
@@ -227,19 +238,18 @@ static int isl29125_buffer_predisable(struct iio_dev *indio_dev)
 	struct isl29125_data *data = iio_priv(indio_dev);
 	int ret;
 
-	ret = iio_triggered_buffer_predisable(indio_dev);
-	if (ret < 0)
-		return ret;
-
 	data->conf1 &= ~ISL29125_MODE_MASK;
 	data->conf1 |= ISL29125_MODE_PD;
-	return i2c_smbus_write_byte_data(data->client, ISL29125_CONF1,
+	ret = i2c_smbus_write_byte_data(data->client, ISL29125_CONF1,
 		data->conf1);
+
+	iio_triggered_buffer_predisable(indio_dev);
+
+	return ret;
 }
 
 static const struct iio_buffer_setup_ops isl29125_buffer_setup_ops = {
-	.preenable = isl29125_buffer_preenable,
-	.postenable = &iio_triggered_buffer_postenable,
+	.postenable = isl29125_buffer_postenable,
 	.predisable = isl29125_buffer_predisable,
 };
 
diff --git a/drivers/iio/light/ltr501.c b/drivers/iio/light/ltr501.c
index 71f99d2a22c1..5a3fcb127cd2 100644
--- a/drivers/iio/light/ltr501.c
+++ b/drivers/iio/light/ltr501.c
@@ -101,12 +101,12 @@ struct ltr501_gain {
 	int uscale;
 };
 
-static struct ltr501_gain ltr501_als_gain_tbl[] = {
+static const struct ltr501_gain ltr501_als_gain_tbl[] = {
 	{1, 0},
 	{0, 5000},
 };
 
-static struct ltr501_gain ltr559_als_gain_tbl[] = {
+static const struct ltr501_gain ltr559_als_gain_tbl[] = {
 	{1, 0},
 	{0, 500000},
 	{0, 250000},
@@ -117,14 +117,14 @@ static struct ltr501_gain ltr559_als_gain_tbl[] = {
 	{0, 10000},
 };
 
-static struct ltr501_gain ltr501_ps_gain_tbl[] = {
+static const struct ltr501_gain ltr501_ps_gain_tbl[] = {
 	{1, 0},
 	{0, 250000},
 	{0, 125000},
 	{0, 62500},
 };
 
-static struct ltr501_gain ltr559_ps_gain_tbl[] = {
+static const struct ltr501_gain ltr559_ps_gain_tbl[] = {
 	{0, 62500}, /* x16 gain */
 	{0, 31250}, /* x32 gain */
 	{0, 15625}, /* bits X1 are for x64 gain */
@@ -133,9 +133,9 @@ static struct ltr501_gain ltr559_ps_gain_tbl[] = {
 
 struct ltr501_chip_info {
 	u8 partid;
-	struct ltr501_gain *als_gain;
+	const struct ltr501_gain *als_gain;
 	int als_gain_tbl_size;
-	struct ltr501_gain *ps_gain;
+	const struct ltr501_gain *ps_gain;
 	int ps_gain_tbl_size;
 	u8 als_mode_active;
 	u8 als_gain_mask;
@@ -192,7 +192,7 @@ static int ltr501_match_samp_freq(const struct ltr501_samp_table *tab,
 	return -EINVAL;
 }
 
-static int ltr501_als_read_samp_freq(struct ltr501_data *data,
+static int ltr501_als_read_samp_freq(const struct ltr501_data *data,
 				     int *val, int *val2)
 {
 	int ret, i;
@@ -210,7 +210,7 @@ static int ltr501_als_read_samp_freq(struct ltr501_data *data,
 	return IIO_VAL_INT_PLUS_MICRO;
 }
 
-static int ltr501_ps_read_samp_freq(struct ltr501_data *data,
+static int ltr501_ps_read_samp_freq(const struct ltr501_data *data,
 				    int *val, int *val2)
 {
 	int ret, i;
@@ -266,7 +266,7 @@ static int ltr501_ps_write_samp_freq(struct ltr501_data *data,
 	return ret;
 }
 
-static int ltr501_als_read_samp_period(struct ltr501_data *data, int *val)
+static int ltr501_als_read_samp_period(const struct ltr501_data *data, int *val)
 {
 	int ret, i;
 
@@ -282,7 +282,7 @@ static int ltr501_als_read_samp_period(struct ltr501_data *data, int *val)
 	return IIO_VAL_INT;
 }
 
-static int ltr501_ps_read_samp_period(struct ltr501_data *data, int *val)
+static int ltr501_ps_read_samp_period(const struct ltr501_data *data, int *val)
 {
 	int ret, i;
 
@@ -321,7 +321,7 @@ static unsigned long ltr501_calculate_lux(u16 vis_data, u16 ir_data)
 	return lux / 1000;
 }
 
-static int ltr501_drdy(struct ltr501_data *data, u8 drdy_mask)
+static int ltr501_drdy(const struct ltr501_data *data, u8 drdy_mask)
 {
 	int tries = 100;
 	int ret, status;
@@ -373,7 +373,8 @@ static int ltr501_set_it_time(struct ltr501_data *data, int it)
 }
 
 /* read int time in micro seconds */
-static int ltr501_read_it_time(struct ltr501_data *data, int *val, int *val2)
+static int ltr501_read_it_time(const struct ltr501_data *data,
+			       int *val, int *val2)
 {
 	int ret, index;
 
@@ -391,7 +392,7 @@ static int ltr501_read_it_time(struct ltr501_data *data, int *val, int *val2)
 	return IIO_VAL_INT_PLUS_MICRO;
 }
 
-static int ltr501_read_als(struct ltr501_data *data, __le16 buf[2])
+static int ltr501_read_als(const struct ltr501_data *data, __le16 buf[2])
 {
 	int ret;
 
@@ -403,7 +404,7 @@ static int ltr501_read_als(struct ltr501_data *data, __le16 buf[2])
 				buf, 2 * sizeof(__le16));
 }
 
-static int ltr501_read_ps(struct ltr501_data *data)
+static int ltr501_read_ps(const struct ltr501_data *data)
 {
 	int ret, status;
 
@@ -419,7 +420,7 @@ static int ltr501_read_ps(struct ltr501_data *data)
 	return status;
 }
 
-static int ltr501_read_intr_prst(struct ltr501_data *data,
+static int ltr501_read_intr_prst(const struct ltr501_data *data,
 				 enum iio_chan_type type,
 				 int *val2)
 {
@@ -716,7 +717,7 @@ static int ltr501_read_raw(struct iio_dev *indio_dev,
 	return -EINVAL;
 }
 
-static int ltr501_get_gain_index(struct ltr501_gain *gain, int size,
+static int ltr501_get_gain_index(const struct ltr501_gain *gain, int size,
 				 int val, int val2)
 {
 	int i;
@@ -848,14 +849,14 @@ static int ltr501_write_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
-static int ltr501_read_thresh(struct iio_dev *indio_dev,
+static int ltr501_read_thresh(const 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 ltr501_data *data = iio_priv(indio_dev);
+	const struct ltr501_data *data = iio_priv(indio_dev);
 	int ret, thresh_data;
 
 	switch (chan->type) {
@@ -1263,7 +1264,7 @@ static irqreturn_t ltr501_trigger_handler(int irq, void *p)
 
 	if (mask & LTR501_STATUS_ALS_RDY) {
 		ret = regmap_bulk_read(data->regmap, LTR501_ALS_DATA1,
-				       (u8 *)als_buf, sizeof(als_buf));
+				       als_buf, sizeof(als_buf));
 		if (ret < 0)
 			return ret;
 		if (test_bit(0, indio_dev->active_scan_mask))
@@ -1359,7 +1360,7 @@ static bool ltr501_is_volatile_reg(struct device *dev, unsigned int reg)
 	}
 }
 
-static struct regmap_config ltr501_regmap_config = {
+static const struct regmap_config ltr501_regmap_config = {
 	.name =  LTR501_REGMAP_NAME,
 	.reg_bits = 8,
 	.val_bits = 8,
diff --git a/drivers/iio/light/opt3001.c b/drivers/iio/light/opt3001.c
index 92004a2563ea..82abfa57b59c 100644
--- a/drivers/iio/light/opt3001.c
+++ b/drivers/iio/light/opt3001.c
@@ -16,6 +16,7 @@
 #include <linux/irq.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
 #include <linux/types.h>
@@ -844,7 +845,7 @@ static struct i2c_driver opt3001_driver = {
 
 	.driver = {
 		.name = "opt3001",
-		.of_match_table = of_match_ptr(opt3001_of_match),
+		.of_match_table = opt3001_of_match,
 	},
 };
 
diff --git a/drivers/iio/light/si1133.c b/drivers/iio/light/si1133.c
index 9174ab928880..c1adab2a50fd 100644
--- a/drivers/iio/light/si1133.c
+++ b/drivers/iio/light/si1133.c
@@ -17,6 +17,8 @@
 
 #include <linux/util_macros.h>
 
+#include <asm/unaligned.h>
+
 #define SI1133_REG_PART_ID		0x00
 #define SI1133_REG_REV_ID		0x01
 #define SI1133_REG_MFR_ID		0x02
@@ -104,8 +106,6 @@
 #define SI1133_LUX_BUFFER_SIZE		9
 #define SI1133_MEASURE_BUFFER_SIZE	3
 
-#define SI1133_SIGN_BIT_INDEX 23
-
 static const int si1133_scale_available[] = {
 	1, 2, 4, 8, 16, 32, 64, 128};
 
@@ -633,8 +633,7 @@ static int si1133_measure(struct si1133_data *data,
 	if (err)
 		return err;
 
-	*val = sign_extend32((buffer[0] << 16) | (buffer[1] << 8) | buffer[2],
-			     SI1133_SIGN_BIT_INDEX);
+	*val = sign_extend32(get_unaligned_be24(&buffer[0]), 23);
 
 	return err;
 }
@@ -723,16 +722,11 @@ static int si1133_get_lux(struct si1133_data *data, int *val)
 	if (err)
 		return err;
 
-	high_vis =
-		sign_extend32((buffer[0] << 16) | (buffer[1] << 8) | buffer[2],
-			      SI1133_SIGN_BIT_INDEX);
+	high_vis = sign_extend32(get_unaligned_be24(&buffer[0]), 23);
 
-	low_vis =
-		sign_extend32((buffer[3] << 16) | (buffer[4] << 8) | buffer[5],
-			      SI1133_SIGN_BIT_INDEX);
+	low_vis = sign_extend32(get_unaligned_be24(&buffer[3]), 23);
 
-	ir = sign_extend32((buffer[6] << 16) | (buffer[7] << 8) | buffer[8],
-			   SI1133_SIGN_BIT_INDEX);
+	ir = sign_extend32(get_unaligned_be24(&buffer[6]), 23);
 
 	if (high_vis > SI1133_ADC_THRESHOLD || ir > SI1133_ADC_THRESHOLD)
 		lux = si1133_calc_polynomial(high_vis, ir,
diff --git a/drivers/iio/light/st_uvis25_i2c.c b/drivers/iio/light/st_uvis25_i2c.c
index 4889bbeb0c73..98cd49eefe45 100644
--- a/drivers/iio/light/st_uvis25_i2c.c
+++ b/drivers/iio/light/st_uvis25_i2c.c
@@ -9,6 +9,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
@@ -31,8 +32,8 @@ static int st_uvis25_i2c_probe(struct i2c_client *client,
 
 	regmap = devm_regmap_init_i2c(client, &st_uvis25_i2c_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&client->dev, "Failed to register i2c regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&client->dev, "Failed to register i2c regmap %ld\n",
+			PTR_ERR(regmap));
 		return PTR_ERR(regmap);
 	}
 
@@ -55,7 +56,7 @@ static struct i2c_driver st_uvis25_driver = {
 	.driver = {
 		.name = "st_uvis25_i2c",
 		.pm = &st_uvis25_pm_ops,
-		.of_match_table = of_match_ptr(st_uvis25_i2c_of_match),
+		.of_match_table = st_uvis25_i2c_of_match,
 	},
 	.probe = st_uvis25_i2c_probe,
 	.id_table = st_uvis25_i2c_id_table,
diff --git a/drivers/iio/light/st_uvis25_spi.c b/drivers/iio/light/st_uvis25_spi.c
index a9ceae4f58b3..af9d94d12787 100644
--- a/drivers/iio/light/st_uvis25_spi.c
+++ b/drivers/iio/light/st_uvis25_spi.c
@@ -9,6 +9,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/spi/spi.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
@@ -31,8 +32,8 @@ static int st_uvis25_spi_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &st_uvis25_spi_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap %ld\n",
+			PTR_ERR(regmap));
 		return PTR_ERR(regmap);
 	}
 
@@ -55,7 +56,7 @@ static struct spi_driver st_uvis25_driver = {
 	.driver = {
 		.name = "st_uvis25_spi",
 		.pm = &st_uvis25_pm_ops,
-		.of_match_table = of_match_ptr(st_uvis25_spi_of_match),
+		.of_match_table = st_uvis25_spi_of_match,
 	},
 	.probe = st_uvis25_spi_probe,
 	.id_table = st_uvis25_spi_id_table,
diff --git a/drivers/iio/light/tsl2563.c b/drivers/iio/light/tsl2563.c
index d8c40a83097d..27a5c28aac7f 100644
--- a/drivers/iio/light/tsl2563.c
+++ b/drivers/iio/light/tsl2563.c
@@ -69,7 +69,7 @@
 #define TSL2563_TIMING_GAIN16	0x10
 #define TSL2563_TIMING_GAIN1	0x00
 
-#define TSL2563_INT_DISBLED	0x00
+#define TSL2563_INT_DISABLED	0x00
 #define TSL2563_INT_LEVEL	0x10
 #define TSL2563_INT_PERSIST(n)	((n) & 0x0F)
 
diff --git a/drivers/iio/light/tsl2772.c b/drivers/iio/light/tsl2772.c
index be37fcbd4654..9fbde9b71b63 100644
--- a/drivers/iio/light/tsl2772.c
+++ b/drivers/iio/light/tsl2772.c
@@ -932,7 +932,7 @@ static ssize_t in_illuminance0_target_input_show(struct device *dev,
 {
 	struct tsl2772_chip *chip = iio_priv(dev_to_iio_dev(dev));
 
-	return snprintf(buf, PAGE_SIZE, "%d\n", chip->settings.als_cal_target);
+	return scnprintf(buf, PAGE_SIZE, "%d\n", chip->settings.als_cal_target);
 }
 
 static ssize_t in_illuminance0_target_input_store(struct device *dev,
@@ -986,7 +986,7 @@ static ssize_t in_illuminance0_lux_table_show(struct device *dev,
 	int offset = 0;
 
 	while (i < TSL2772_MAX_LUX_TABLE_SIZE) {
-		offset += snprintf(buf + offset, PAGE_SIZE, "%u,%u,",
+		offset += scnprintf(buf + offset, PAGE_SIZE - offset, "%u,%u,",
 			chip->tsl2772_device_lux[i].ch0,
 			chip->tsl2772_device_lux[i].ch1);
 		if (chip->tsl2772_device_lux[i].ch0 == 0) {
@@ -1000,7 +1000,7 @@ static ssize_t in_illuminance0_lux_table_show(struct device *dev,
 		i++;
 	}
 
-	offset += snprintf(buf + offset, PAGE_SIZE, "\n");
+	offset += scnprintf(buf + offset, PAGE_SIZE - offset, "\n");
 	return offset;
 }
 
diff --git a/drivers/iio/light/vcnl4000.c b/drivers/iio/light/vcnl4000.c
index ec803c1e81df..2a4b3d331055 100644
--- a/drivers/iio/light/vcnl4000.c
+++ b/drivers/iio/light/vcnl4000.c
@@ -5,6 +5,7 @@
  *
  * Copyright 2012 Peter Meerwald <pmeerw@pmeerw.net>
  * Copyright 2019 Pursim SPC
+ * Copyright 2020 Mathieu Othacehe <m.othacehe@gmail.com>
  *
  * IIO driver for:
  *   VCNL4000/10/20 (7-bit I2C slave address 0x13)
@@ -13,9 +14,7 @@
  *
  * TODO:
  *   allow to adjust IR current
- *   proximity threshold and event handling
- *   periodic ALS/proximity measurement (VCNL4010/20)
- *   interrupts (VCNL4010/20/40, VCNL4200)
+ *   interrupts (VCNL4040, VCNL4200)
  */
 
 #include <linux/module.h>
@@ -23,9 +22,15 @@
 #include <linux/err.h>
 #include <linux/delay.h>
 #include <linux/pm_runtime.h>
+#include <linux/interrupt.h>
 
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
 
 #define VCNL4000_DRV_NAME "vcnl4000"
 #define VCNL4000_PROD_ID	0x01
@@ -35,14 +40,22 @@
 
 #define VCNL4000_COMMAND	0x80 /* Command register */
 #define VCNL4000_PROD_REV	0x81 /* Product ID and Revision ID */
+#define VCNL4010_PROX_RATE      0x82 /* Proximity rate */
 #define VCNL4000_LED_CURRENT	0x83 /* IR LED current for proximity mode */
 #define VCNL4000_AL_PARAM	0x84 /* Ambient light parameter register */
+#define VCNL4010_ALS_PARAM      0x84 /* ALS rate */
 #define VCNL4000_AL_RESULT_HI	0x85 /* Ambient light result register, MSB */
 #define VCNL4000_AL_RESULT_LO	0x86 /* Ambient light result register, LSB */
 #define VCNL4000_PS_RESULT_HI	0x87 /* Proximity result register, MSB */
 #define VCNL4000_PS_RESULT_LO	0x88 /* Proximity result register, LSB */
 #define VCNL4000_PS_MEAS_FREQ	0x89 /* Proximity test signal frequency */
+#define VCNL4010_INT_CTRL	0x89 /* Interrupt control */
 #define VCNL4000_PS_MOD_ADJ	0x8a /* Proximity modulator timing adjustment */
+#define VCNL4010_LOW_THR_HI     0x8a /* Low threshold, MSB */
+#define VCNL4010_LOW_THR_LO     0x8b /* Low threshold, LSB */
+#define VCNL4010_HIGH_THR_HI    0x8c /* High threshold, MSB */
+#define VCNL4010_HIGH_THR_LO    0x8d /* High threshold, LSB */
+#define VCNL4010_ISR		0x8e /* Interrupt status */
 
 #define VCNL4200_AL_CONF	0x00 /* Ambient light configuration */
 #define VCNL4200_PS_CONF1	0x03 /* Proximity configuration */
@@ -57,6 +70,36 @@
 #define VCNL4000_PS_RDY		BIT(5) /* proximity data ready? */
 #define VCNL4000_AL_OD		BIT(4) /* start on-demand ALS measurement */
 #define VCNL4000_PS_OD		BIT(3) /* start on-demand proximity measurement */
+#define VCNL4000_ALS_EN		BIT(2) /* start ALS measurement */
+#define VCNL4000_PROX_EN	BIT(1) /* start proximity measurement */
+#define VCNL4000_SELF_TIMED_EN	BIT(0) /* start self-timed measurement */
+
+/* Bit masks for interrupt registers. */
+#define VCNL4010_INT_THR_SEL	BIT(0) /* Select threshold interrupt source */
+#define VCNL4010_INT_THR_EN	BIT(1) /* Threshold interrupt type */
+#define VCNL4010_INT_ALS_EN	BIT(2) /* Enable on ALS data ready */
+#define VCNL4010_INT_PROX_EN	BIT(3) /* Enable on proximity data ready */
+
+#define VCNL4010_INT_THR_HIGH	0 /* High threshold exceeded */
+#define VCNL4010_INT_THR_LOW	1 /* Low threshold exceeded */
+#define VCNL4010_INT_ALS	2 /* ALS data ready */
+#define VCNL4010_INT_PROXIMITY	3 /* Proximity data ready */
+
+#define VCNL4010_INT_THR \
+	(BIT(VCNL4010_INT_THR_LOW) | BIT(VCNL4010_INT_THR_HIGH))
+#define VCNL4010_INT_DRDY \
+	(BIT(VCNL4010_INT_PROXIMITY) | BIT(VCNL4010_INT_ALS))
+
+static const int vcnl4010_prox_sampling_frequency[][2] = {
+	{1, 950000},
+	{3, 906250},
+	{7, 812500},
+	{16, 625000},
+	{31, 250000},
+	{62, 500000},
+	{125, 0},
+	{250, 0},
+};
 
 #define VCNL4000_SLEEP_DELAY_MS	2000 /* before we enter pm_runtime_suspend */
 
@@ -83,10 +126,15 @@ struct vcnl4000_data {
 	struct mutex vcnl4000_lock;
 	struct vcnl4200_channel vcnl4200_al;
 	struct vcnl4200_channel vcnl4200_ps;
+	uint32_t near_level;
 };
 
 struct vcnl4000_chip_spec {
 	const char *prod;
+	struct iio_chan_spec const *channels;
+	const int num_channels;
+	const struct iio_info *info;
+	bool irq_support;
 	int (*init)(struct vcnl4000_data *data);
 	int (*measure_light)(struct vcnl4000_data *data, int *val);
 	int (*measure_proximity)(struct vcnl4000_data *data, int *val);
@@ -215,11 +263,31 @@ static int vcnl4200_init(struct vcnl4000_data *data)
 	return 0;
 };
 
+static int vcnl4000_read_data(struct vcnl4000_data *data, u8 data_reg, int *val)
+{
+	s32 ret;
+
+	ret = i2c_smbus_read_word_swapped(data->client, data_reg);
+	if (ret < 0)
+		return ret;
+
+	*val = ret;
+	return 0;
+}
+
+static int vcnl4000_write_data(struct vcnl4000_data *data, u8 data_reg, int val)
+{
+	if (val > U16_MAX)
+		return -ERANGE;
+
+	return i2c_smbus_write_word_swapped(data->client, data_reg, val);
+}
+
+
 static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
 				u8 rdy_mask, u8 data_reg, int *val)
 {
 	int tries = 20;
-	__be16 buf;
 	int ret;
 
 	mutex_lock(&data->vcnl4000_lock);
@@ -246,13 +314,11 @@ static int vcnl4000_measure(struct vcnl4000_data *data, u8 req_mask,
 		goto fail;
 	}
 
-	ret = i2c_smbus_read_i2c_block_data(data->client,
-		data_reg, sizeof(buf), (u8 *) &buf);
+	ret = vcnl4000_read_data(data, data_reg, val);
 	if (ret < 0)
 		goto fail;
 
 	mutex_unlock(&data->vcnl4000_lock);
-	*val = be16_to_cpu(buf);
 
 	return 0;
 
@@ -312,47 +378,34 @@ static int vcnl4200_measure_proximity(struct vcnl4000_data *data, int *val)
 	return vcnl4200_measure(data, &data->vcnl4200_ps, val);
 }
 
-static const struct vcnl4000_chip_spec vcnl4000_chip_spec_cfg[] = {
-	[VCNL4000] = {
-		.prod = "VCNL4000",
-		.init = vcnl4000_init,
-		.measure_light = vcnl4000_measure_light,
-		.measure_proximity = vcnl4000_measure_proximity,
-		.set_power_state = vcnl4000_set_power_state,
-	},
-	[VCNL4010] = {
-		.prod = "VCNL4010/4020",
-		.init = vcnl4000_init,
-		.measure_light = vcnl4000_measure_light,
-		.measure_proximity = vcnl4000_measure_proximity,
-		.set_power_state = vcnl4000_set_power_state,
-	},
-	[VCNL4040] = {
-		.prod = "VCNL4040",
-		.init = vcnl4200_init,
-		.measure_light = vcnl4200_measure_light,
-		.measure_proximity = vcnl4200_measure_proximity,
-		.set_power_state = vcnl4200_set_power_state,
-	},
-	[VCNL4200] = {
-		.prod = "VCNL4200",
-		.init = vcnl4200_init,
-		.measure_light = vcnl4200_measure_light,
-		.measure_proximity = vcnl4200_measure_proximity,
-		.set_power_state = vcnl4200_set_power_state,
-	},
-};
+static int vcnl4010_read_proxy_samp_freq(struct vcnl4000_data *data, int *val,
+					 int *val2)
+{
+	int ret;
 
-static const struct iio_chan_spec vcnl4000_channels[] = {
-	{
-		.type = IIO_LIGHT,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
-			BIT(IIO_CHAN_INFO_SCALE),
-	}, {
-		.type = IIO_PROXIMITY,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
-	}
-};
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4010_PROX_RATE);
+	if (ret < 0)
+		return ret;
+
+	if (ret >= ARRAY_SIZE(vcnl4010_prox_sampling_frequency))
+		return -EINVAL;
+
+	*val = vcnl4010_prox_sampling_frequency[ret][0];
+	*val2 = vcnl4010_prox_sampling_frequency[ret][1];
+
+	return 0;
+}
+
+static bool vcnl4010_is_in_periodic_mode(struct vcnl4000_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4000_COMMAND);
+	if (ret < 0)
+		return false;
+
+	return !!(ret & VCNL4000_SELF_TIMED_EN);
+}
 
 static int vcnl4000_set_pm_runtime_state(struct vcnl4000_data *data, bool on)
 {
@@ -412,10 +465,571 @@ static int vcnl4000_read_raw(struct iio_dev *indio_dev,
 	}
 }
 
+static int vcnl4010_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int *val, int *val2, long mask)
+{
+	int ret;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+	case IIO_CHAN_INFO_SCALE:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		/* Protect against event capture. */
+		if (vcnl4010_is_in_periodic_mode(data)) {
+			ret = -EBUSY;
+		} else {
+			ret = vcnl4000_read_raw(indio_dev, chan, val, val2,
+						mask);
+		}
+
+		iio_device_release_direct_mode(indio_dev);
+		return ret;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		switch (chan->type) {
+		case IIO_PROXIMITY:
+			ret = vcnl4010_read_proxy_samp_freq(data, val, val2);
+			if (ret < 0)
+				return ret;
+			return IIO_VAL_INT_PLUS_MICRO;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int vcnl4010_read_avail(struct iio_dev *indio_dev,
+			       struct iio_chan_spec const *chan,
+			       const int **vals, int *type, int *length,
+			       long mask)
+{
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*vals = (int *)vcnl4010_prox_sampling_frequency;
+		*type = IIO_VAL_INT_PLUS_MICRO;
+		*length = 2 * ARRAY_SIZE(vcnl4010_prox_sampling_frequency);
+		return IIO_AVAIL_LIST;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int vcnl4010_write_proxy_samp_freq(struct vcnl4000_data *data, int val,
+					  int val2)
+{
+	unsigned int i;
+	int index = -1;
+
+	for (i = 0; i < ARRAY_SIZE(vcnl4010_prox_sampling_frequency); i++) {
+		if (val == vcnl4010_prox_sampling_frequency[i][0] &&
+		    val2 == vcnl4010_prox_sampling_frequency[i][1]) {
+			index = i;
+			break;
+		}
+	}
+
+	if (index < 0)
+		return -EINVAL;
+
+	return i2c_smbus_write_byte_data(data->client, VCNL4010_PROX_RATE,
+					 index);
+}
+
+static int vcnl4010_write_raw(struct iio_dev *indio_dev,
+			      struct iio_chan_spec const *chan,
+			      int val, int val2, long mask)
+{
+	int ret;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	ret = iio_device_claim_direct_mode(indio_dev);
+	if (ret)
+		return ret;
+
+	/* Protect against event capture. */
+	if (vcnl4010_is_in_periodic_mode(data)) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		switch (chan->type) {
+		case IIO_PROXIMITY:
+			ret = vcnl4010_write_proxy_samp_freq(data, val, val2);
+			goto end;
+		default:
+			ret = -EINVAL;
+			goto end;
+		}
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+
+end:
+	iio_device_release_direct_mode(indio_dev);
+	return ret;
+}
+
+static int vcnl4010_read_event(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)
+{
+	int ret;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			ret = vcnl4000_read_data(data, VCNL4010_HIGH_THR_HI,
+						 val);
+			if (ret < 0)
+				return ret;
+			return IIO_VAL_INT;
+		case IIO_EV_DIR_FALLING:
+			ret = vcnl4000_read_data(data, VCNL4010_LOW_THR_HI,
+						 val);
+			if (ret < 0)
+				return ret;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static int vcnl4010_write_event(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)
+{
+	int ret;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		switch (dir) {
+		case IIO_EV_DIR_RISING:
+			ret = vcnl4000_write_data(data, VCNL4010_HIGH_THR_HI,
+						  val);
+			if (ret < 0)
+				return ret;
+			return IIO_VAL_INT;
+		case IIO_EV_DIR_FALLING:
+			ret = vcnl4000_write_data(data, VCNL4010_LOW_THR_HI,
+						  val);
+			if (ret < 0)
+				return ret;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	default:
+		return -EINVAL;
+	}
+}
+
+static bool vcnl4010_is_thr_enabled(struct vcnl4000_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4010_INT_CTRL);
+	if (ret < 0)
+		return false;
+
+	return !!(ret & VCNL4010_INT_THR_EN);
+}
+
+static int vcnl4010_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 vcnl4000_data *data = iio_priv(indio_dev);
+
+	switch (chan->type) {
+	case IIO_PROXIMITY:
+		return vcnl4010_is_thr_enabled(data);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int vcnl4010_config_threshold(struct iio_dev *indio_dev, bool state)
+{
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	int ret;
+	int icr;
+	int command;
+
+	if (state) {
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		/* Enable periodic measurement of proximity data. */
+		command = VCNL4000_SELF_TIMED_EN | VCNL4000_PROX_EN;
+
+		/*
+		 * Enable interrupts on threshold, for proximity data by
+		 * default.
+		 */
+		icr = VCNL4010_INT_THR_EN;
+	} else {
+		if (!vcnl4010_is_thr_enabled(data))
+			return 0;
+
+		command = 0;
+		icr = 0;
+	}
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4000_COMMAND,
+					command);
+	if (ret < 0)
+		goto end;
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4010_INT_CTRL, icr);
+
+end:
+	if (state)
+		iio_device_release_direct_mode(indio_dev);
+
+	return ret;
+}
+
+static int vcnl4010_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)
+{
+	switch (chan->type) {
+	case IIO_PROXIMITY:
+		return vcnl4010_config_threshold(indio_dev, state);
+	default:
+		return -EINVAL;
+	}
+}
+
+static ssize_t vcnl4000_read_near_level(struct iio_dev *indio_dev,
+					uintptr_t priv,
+					const struct iio_chan_spec *chan,
+					char *buf)
+{
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+
+	return sprintf(buf, "%u\n", data->near_level);
+}
+
+static const struct iio_chan_spec_ext_info vcnl4000_ext_info[] = {
+	{
+		.name = "nearlevel",
+		.shared = IIO_SEPARATE,
+		.read = vcnl4000_read_near_level,
+	},
+	{ /* sentinel */ }
+};
+
+static const struct iio_event_spec vcnl4000_event_spec[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE),
+	}, {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_EITHER,
+		.mask_separate = BIT(IIO_EV_INFO_ENABLE),
+	}
+};
+
+static const struct iio_chan_spec vcnl4000_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
+	}, {
+		.type = IIO_PROXIMITY,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+		.ext_info = vcnl4000_ext_info,
+	}
+};
+
+static const struct iio_chan_spec vcnl4010_channels[] = {
+	{
+		.type = IIO_LIGHT,
+		.scan_index = -1,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SCALE),
+	}, {
+		.type = IIO_PROXIMITY,
+		.scan_index = 0,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+			BIT(IIO_CHAN_INFO_SAMP_FREQ),
+		.info_mask_separate_available = BIT(IIO_CHAN_INFO_SAMP_FREQ),
+		.event_spec = vcnl4000_event_spec,
+		.num_event_specs = ARRAY_SIZE(vcnl4000_event_spec),
+		.ext_info = vcnl4000_ext_info,
+		.scan_type = {
+			.sign = 'u',
+			.realbits = 16,
+			.storagebits = 16,
+			.endianness = IIO_CPU,
+		},
+	},
+	IIO_CHAN_SOFT_TIMESTAMP(1),
+};
+
 static const struct iio_info vcnl4000_info = {
 	.read_raw = vcnl4000_read_raw,
 };
 
+static const struct iio_info vcnl4010_info = {
+	.read_raw = vcnl4010_read_raw,
+	.read_avail = vcnl4010_read_avail,
+	.write_raw = vcnl4010_write_raw,
+	.read_event_value = vcnl4010_read_event,
+	.write_event_value = vcnl4010_write_event,
+	.read_event_config = vcnl4010_read_event_config,
+	.write_event_config = vcnl4010_write_event_config,
+};
+
+static const struct vcnl4000_chip_spec vcnl4000_chip_spec_cfg[] = {
+	[VCNL4000] = {
+		.prod = "VCNL4000",
+		.init = vcnl4000_init,
+		.measure_light = vcnl4000_measure_light,
+		.measure_proximity = vcnl4000_measure_proximity,
+		.set_power_state = vcnl4000_set_power_state,
+		.channels = vcnl4000_channels,
+		.num_channels = ARRAY_SIZE(vcnl4000_channels),
+		.info = &vcnl4000_info,
+		.irq_support = false,
+	},
+	[VCNL4010] = {
+		.prod = "VCNL4010/4020",
+		.init = vcnl4000_init,
+		.measure_light = vcnl4000_measure_light,
+		.measure_proximity = vcnl4000_measure_proximity,
+		.set_power_state = vcnl4000_set_power_state,
+		.channels = vcnl4010_channels,
+		.num_channels = ARRAY_SIZE(vcnl4010_channels),
+		.info = &vcnl4010_info,
+		.irq_support = true,
+	},
+	[VCNL4040] = {
+		.prod = "VCNL4040",
+		.init = vcnl4200_init,
+		.measure_light = vcnl4200_measure_light,
+		.measure_proximity = vcnl4200_measure_proximity,
+		.set_power_state = vcnl4200_set_power_state,
+		.channels = vcnl4000_channels,
+		.num_channels = ARRAY_SIZE(vcnl4000_channels),
+		.info = &vcnl4000_info,
+		.irq_support = false,
+	},
+	[VCNL4200] = {
+		.prod = "VCNL4200",
+		.init = vcnl4200_init,
+		.measure_light = vcnl4200_measure_light,
+		.measure_proximity = vcnl4200_measure_proximity,
+		.set_power_state = vcnl4200_set_power_state,
+		.channels = vcnl4000_channels,
+		.num_channels = ARRAY_SIZE(vcnl4000_channels),
+		.info = &vcnl4000_info,
+		.irq_support = false,
+	},
+};
+
+static irqreturn_t vcnl4010_irq_thread(int irq, void *p)
+{
+	struct iio_dev *indio_dev = p;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	unsigned long isr;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4010_ISR);
+	if (ret < 0)
+		goto end;
+
+	isr = ret;
+
+	if (isr & VCNL4010_INT_THR) {
+		if (test_bit(VCNL4010_INT_THR_LOW, &isr)) {
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(
+					       IIO_PROXIMITY,
+					       1,
+					       IIO_EV_TYPE_THRESH,
+					       IIO_EV_DIR_FALLING),
+				       iio_get_time_ns(indio_dev));
+		}
+
+		if (test_bit(VCNL4010_INT_THR_HIGH, &isr)) {
+			iio_push_event(indio_dev,
+				       IIO_UNMOD_EVENT_CODE(
+					       IIO_PROXIMITY,
+					       1,
+					       IIO_EV_TYPE_THRESH,
+					       IIO_EV_DIR_RISING),
+				       iio_get_time_ns(indio_dev));
+		}
+
+		i2c_smbus_write_byte_data(data->client, VCNL4010_ISR,
+					  isr & VCNL4010_INT_THR);
+	}
+
+	if (isr & VCNL4010_INT_DRDY && iio_buffer_enabled(indio_dev))
+		iio_trigger_poll_chained(indio_dev->trig);
+
+end:
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t vcnl4010_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	const unsigned long *active_scan_mask = indio_dev->active_scan_mask;
+	u16 buffer[8] = {0}; /* 1x16-bit + ts */
+	bool data_read = false;
+	unsigned long isr;
+	int val = 0;
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, VCNL4010_ISR);
+	if (ret < 0)
+		goto end;
+
+	isr = ret;
+
+	if (test_bit(0, active_scan_mask)) {
+		if (test_bit(VCNL4010_INT_PROXIMITY, &isr)) {
+			ret = vcnl4000_read_data(data,
+						 VCNL4000_PS_RESULT_HI,
+						 &val);
+			if (ret < 0)
+				goto end;
+
+			buffer[0] = val;
+			data_read = true;
+		}
+	}
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4010_ISR,
+					isr & VCNL4010_INT_DRDY);
+	if (ret < 0)
+		goto end;
+
+	if (!data_read)
+		goto end;
+
+	iio_push_to_buffers_with_timestamp(indio_dev, buffer,
+					   iio_get_time_ns(indio_dev));
+
+end:
+	iio_trigger_notify_done(indio_dev->trig);
+	return IRQ_HANDLED;
+}
+
+static int vcnl4010_buffer_postenable(struct iio_dev *indio_dev)
+{
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	int ret;
+	int cmd;
+
+	ret = iio_triggered_buffer_postenable(indio_dev);
+	if (ret)
+		return ret;
+
+	/* Do not enable the buffer if we are already capturing events. */
+	if (vcnl4010_is_in_periodic_mode(data)) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4010_INT_CTRL,
+					VCNL4010_INT_PROX_EN);
+	if (ret < 0)
+		goto end;
+
+	cmd = VCNL4000_SELF_TIMED_EN | VCNL4000_PROX_EN;
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4000_COMMAND, cmd);
+	if (ret < 0)
+		goto end;
+
+	return 0;
+end:
+	iio_triggered_buffer_predisable(indio_dev);
+
+	return ret;
+}
+
+static int vcnl4010_buffer_predisable(struct iio_dev *indio_dev)
+{
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	int ret, ret_disable;
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4010_INT_CTRL, 0);
+	if (ret < 0)
+		goto end;
+
+	ret = i2c_smbus_write_byte_data(data->client, VCNL4000_COMMAND, 0);
+
+end:
+	ret_disable = iio_triggered_buffer_predisable(indio_dev);
+	if (ret == 0)
+		ret = ret_disable;
+
+	return ret;
+}
+
+static const struct iio_buffer_setup_ops vcnl4010_buffer_ops = {
+	.postenable = &vcnl4010_buffer_postenable,
+	.predisable = &vcnl4010_buffer_predisable,
+};
+
+static const struct iio_trigger_ops vcnl4010_trigger_ops = {
+	.validate_device = iio_trigger_validate_own_device,
+};
+
+static int vcnl4010_probe_trigger(struct iio_dev *indio_dev)
+{
+	struct vcnl4000_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	struct iio_trigger *trigger;
+
+	trigger = devm_iio_trigger_alloc(&client->dev, "%s-dev%d",
+					 indio_dev->name, indio_dev->id);
+	if (!trigger)
+		return -ENOMEM;
+
+	trigger->dev.parent = &client->dev;
+	trigger->ops = &vcnl4010_trigger_ops;
+	iio_trigger_set_drvdata(trigger, indio_dev);
+
+	return devm_iio_trigger_register(&client->dev, trigger);
+}
+
 static int vcnl4000_probe(struct i2c_client *client,
 			  const struct i2c_device_id *id)
 {
@@ -440,13 +1054,44 @@ static int vcnl4000_probe(struct i2c_client *client,
 	dev_dbg(&client->dev, "%s Ambient light/proximity sensor, Rev: %02x\n",
 		data->chip_spec->prod, data->rev);
 
+	if (device_property_read_u32(&client->dev, "proximity-near-level",
+				     &data->near_level))
+		data->near_level = 0;
+
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->info = &vcnl4000_info;
-	indio_dev->channels = vcnl4000_channels;
-	indio_dev->num_channels = ARRAY_SIZE(vcnl4000_channels);
+	indio_dev->info = data->chip_spec->info;
+	indio_dev->channels = data->chip_spec->channels;
+	indio_dev->num_channels = data->chip_spec->num_channels;
 	indio_dev->name = VCNL4000_DRV_NAME;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
+	if (client->irq && data->chip_spec->irq_support) {
+		ret = devm_iio_triggered_buffer_setup(&client->dev, indio_dev,
+						      NULL,
+						      vcnl4010_trigger_handler,
+						      &vcnl4010_buffer_ops);
+		if (ret < 0) {
+			dev_err(&client->dev,
+				"unable to setup iio triggered buffer\n");
+			return ret;
+		}
+
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						NULL, vcnl4010_irq_thread,
+						IRQF_TRIGGER_FALLING |
+						IRQF_ONESHOT,
+						"vcnl4010_irq",
+						indio_dev);
+		if (ret < 0) {
+			dev_err(&client->dev, "irq request failed\n");
+			return ret;
+		}
+
+		ret = vcnl4010_probe_trigger(indio_dev);
+		if (ret < 0)
+			return ret;
+	}
+
 	ret = pm_runtime_set_active(&client->dev);
 	if (ret < 0)
 		goto fail_poweroff;
@@ -540,5 +1185,6 @@ static struct i2c_driver vcnl4000_driver = {
 module_i2c_driver(vcnl4000_driver);
 
 MODULE_AUTHOR("Peter Meerwald <pmeerw@pmeerw.net>");
+MODULE_AUTHOR("Mathieu Othacehe <m.othacehe@gmail.com>");
 MODULE_DESCRIPTION("Vishay VCNL4000 proximity/ambient light sensor driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/light/vl6180.c b/drivers/iio/light/vl6180.c
index d9533a76b8f6..ed7b02765b97 100644
--- a/drivers/iio/light/vl6180.c
+++ b/drivers/iio/light/vl6180.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/mod_devicetable.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
 #include <linux/err.h>
@@ -537,7 +538,7 @@ 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),
+		.of_match_table = vl6180_of_match,
 	},
 	.probe  = vl6180_probe,
 	.id_table = vl6180_id,
diff --git a/drivers/iio/light/zopt2201.c b/drivers/iio/light/zopt2201.c
index 5f54f39e7a4c..80ae530720cd 100644
--- a/drivers/iio/light/zopt2201.c
+++ b/drivers/iio/light/zopt2201.c
@@ -19,6 +19,8 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 
+#include <asm/unaligned.h>
+
 #define ZOPT2201_DRV_NAME "zopt2201"
 
 /* Registers */
@@ -219,7 +221,7 @@ static int zopt2201_read(struct zopt2201_data *data, u8 reg)
 		goto fail;
 	mutex_unlock(&data->lock);
 
-	return (buf[2] << 16) | (buf[1] << 8) | buf[0];
+	return get_unaligned_le24(&buf[0]);
 
 fail:
 	mutex_unlock(&data->lock);
diff --git a/drivers/iio/magnetometer/ak8974.c b/drivers/iio/magnetometer/ak8974.c
index d32996702110..810fdfd37c88 100644
--- a/drivers/iio/magnetometer/ak8974.c
+++ b/drivers/iio/magnetometer/ak8974.c
@@ -49,6 +49,7 @@
 #define AK8974_WHOAMI_VALUE_AMI306 0x46
 #define AK8974_WHOAMI_VALUE_AMI305 0x47
 #define AK8974_WHOAMI_VALUE_AK8974 0x48
+#define AK8974_WHOAMI_VALUE_HSCDTD008A 0x49
 
 #define AK8974_DATA_X		0x10
 #define AK8974_DATA_Y		0x12
@@ -140,6 +141,12 @@
 #define AK8974_INT_CTRL_PULSE	BIT(1) /* 0 = latched; 1 = pulse (50 usec) */
 #define AK8974_INT_CTRL_RESDEF	(AK8974_INT_CTRL_XYZEN | AK8974_INT_CTRL_POL)
 
+/* HSCDTD008A-specific control register */
+#define HSCDTD008A_CTRL4	0x1E
+#define HSCDTD008A_CTRL4_MMD	BIT(7)	/* must be set to 1 */
+#define HSCDTD008A_CTRL4_RANGE	BIT(4)	/* 0 = 14-bit output; 1 = 15-bit output */
+#define HSCDTD008A_CTRL4_RESDEF	(HSCDTD008A_CTRL4_MMD | HSCDTD008A_CTRL4_RANGE)
+
 /* The AMI305 has elaborate FW version and serial number registers */
 #define AMI305_VER		0xE8
 #define AMI305_SN		0xEA
@@ -241,10 +248,17 @@ static int ak8974_reset(struct ak8974 *ak8974)
 	ret = regmap_write(ak8974->map, AK8974_CTRL3, AK8974_CTRL3_RESDEF);
 	if (ret)
 		return ret;
-	ret = regmap_write(ak8974->map, AK8974_INT_CTRL,
-			   AK8974_INT_CTRL_RESDEF);
-	if (ret)
-		return ret;
+	if (ak8974->variant != AK8974_WHOAMI_VALUE_HSCDTD008A) {
+		ret = regmap_write(ak8974->map, AK8974_INT_CTRL,
+				   AK8974_INT_CTRL_RESDEF);
+		if (ret)
+			return ret;
+	} else {
+		ret = regmap_write(ak8974->map, HSCDTD008A_CTRL4,
+				   HSCDTD008A_CTRL4_RESDEF);
+		if (ret)
+			return ret;
+	}
 
 	/* After reset, power off is default state */
 	return ak8974_set_power(ak8974, AK8974_PWR_OFF);
@@ -267,6 +281,8 @@ static int ak8974_configure(struct ak8974 *ak8974)
 		if (ret)
 			return ret;
 	}
+	if (ak8974->variant == AK8974_WHOAMI_VALUE_HSCDTD008A)
+		return 0;
 	ret = regmap_write(ak8974->map, AK8974_INT_CTRL, AK8974_INT_CTRL_POL);
 	if (ret)
 		return ret;
@@ -495,6 +511,10 @@ static int ak8974_detect(struct ak8974 *ak8974)
 		name = "ak8974";
 		dev_info(&ak8974->i2c->dev, "detected AK8974\n");
 		break;
+	case AK8974_WHOAMI_VALUE_HSCDTD008A:
+		name = "hscdtd008a";
+		dev_info(&ak8974->i2c->dev, "detected hscdtd008a\n");
+		break;
 	default:
 		dev_err(&ak8974->i2c->dev, "unsupported device (%02x) ",
 			whoami);
@@ -534,47 +554,103 @@ static int ak8974_detect(struct ak8974 *ak8974)
 	return 0;
 }
 
+static int ak8974_measure_channel(struct ak8974 *ak8974, unsigned long address,
+				  int *val)
+{
+	__le16 hw_values[3];
+	int ret;
+
+	pm_runtime_get_sync(&ak8974->i2c->dev);
+	mutex_lock(&ak8974->lock);
+
+	/*
+	 * We read all axes and discard all but one, for optimized
+	 * reading, use the triggered buffer.
+	 */
+	ret = ak8974_trigmeas(ak8974);
+	if (ret)
+		goto out_unlock;
+	ret = ak8974_getresult(ak8974, hw_values);
+	if (ret)
+		goto out_unlock;
+	/*
+	 * This explicit cast to (s16) is necessary as the measurement
+	 * is done in 2's complement with positive and negative values.
+	 * The follwing assignment to *val will then convert the signed
+	 * s16 value to a signed int value.
+	 */
+	*val = (s16)le16_to_cpu(hw_values[address]);
+out_unlock:
+	mutex_unlock(&ak8974->lock);
+	pm_runtime_mark_last_busy(&ak8974->i2c->dev);
+	pm_runtime_put_autosuspend(&ak8974->i2c->dev);
+
+	return ret;
+}
+
 static int ak8974_read_raw(struct iio_dev *indio_dev,
 			   struct iio_chan_spec const *chan,
 			   int *val, int *val2,
 			   long mask)
 {
 	struct ak8974 *ak8974 = iio_priv(indio_dev);
-	__le16 hw_values[3];
-	int ret = -EINVAL;
-
-	pm_runtime_get_sync(&ak8974->i2c->dev);
-	mutex_lock(&ak8974->lock);
+	int ret;
 
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		if (chan->address > 2) {
 			dev_err(&ak8974->i2c->dev, "faulty channel address\n");
-			ret = -EIO;
-			goto out_unlock;
+			return -EIO;
 		}
-		ret = ak8974_trigmeas(ak8974);
-		if (ret)
-			goto out_unlock;
-		ret = ak8974_getresult(ak8974, hw_values);
+		ret = ak8974_measure_channel(ak8974, chan->address, val);
 		if (ret)
-			goto out_unlock;
-
-		/*
-		 * We read all axes and discard all but one, for optimized
-		 * reading, use the triggered buffer.
-		 */
-		*val = (s16)le16_to_cpu(hw_values[chan->address]);
-
-		ret = IIO_VAL_INT;
+			return ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		switch (ak8974->variant) {
+		case AK8974_WHOAMI_VALUE_AMI306:
+		case AK8974_WHOAMI_VALUE_AMI305:
+			/*
+			 * The datasheet for AMI305 and AMI306, page 6
+			 * specifies the range of the sensor to be
+			 * +/- 12 Gauss.
+			 */
+			*val = 12;
+			/*
+			 * 12 bits are used, +/- 2^11
+			 * [ -2048 .. 2047 ] (manual page 20)
+			 * [ 0xf800 .. 0x07ff ]
+			 */
+			*val2 = 11;
+			return IIO_VAL_FRACTIONAL_LOG2;
+		case AK8974_WHOAMI_VALUE_HSCDTD008A:
+			/*
+			 * The datasheet for HSCDTF008A, page 3 specifies the
+			 * range of the sensor as +/- 2.4 mT per axis, which
+			 * corresponds to +/- 2400 uT = +/- 24 Gauss.
+			 */
+			*val = 24;
+			/*
+			 * 15 bits are used (set up in CTRL4), +/- 2^14
+			 * [ -16384 .. 16383 ] (manual page 24)
+			 * [ 0xc000 .. 0x3fff ]
+			 */
+			*val2 = 14;
+			return IIO_VAL_FRACTIONAL_LOG2;
+		default:
+			/* GUESSING +/- 12 Gauss */
+			*val = 12;
+			/* GUESSING 12 bits ADC +/- 2^11 */
+			*val2 = 11;
+			return IIO_VAL_FRACTIONAL_LOG2;
+		}
+		break;
+	default:
+		/* Unknown request */
+		break;
 	}
 
- out_unlock:
-	mutex_unlock(&ak8974->lock);
-	pm_runtime_mark_last_busy(&ak8974->i2c->dev);
-	pm_runtime_put_autosuspend(&ak8974->i2c->dev);
-
-	return ret;
+	return -EINVAL;
 }
 
 static void ak8974_fill_buffer(struct iio_dev *indio_dev)
@@ -631,27 +707,44 @@ static const struct iio_chan_spec_ext_info ak8974_ext_info[] = {
 	{ },
 };
 
-#define AK8974_AXIS_CHANNEL(axis, index)				\
+#define AK8974_AXIS_CHANNEL(axis, index, bits)				\
 	{								\
 		.type = IIO_MAGN,					\
 		.modified = 1,						\
 		.channel2 = IIO_MOD_##axis,				\
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+			BIT(IIO_CHAN_INFO_SCALE),			\
 		.ext_info = ak8974_ext_info,				\
 		.address = index,					\
 		.scan_index = index,					\
 		.scan_type = {						\
 			.sign = 's',					\
-			.realbits = 16,					\
+			.realbits = bits,				\
 			.storagebits = 16,				\
 			.endianness = IIO_LE				\
 		},							\
 	}
 
-static const struct iio_chan_spec ak8974_channels[] = {
-	AK8974_AXIS_CHANNEL(X, 0),
-	AK8974_AXIS_CHANNEL(Y, 1),
-	AK8974_AXIS_CHANNEL(Z, 2),
+/*
+ * We have no datasheet for the AK8974 but we guess that its
+ * ADC is 12 bits. The AMI305 and AMI306 certainly has 12bit
+ * ADC.
+ */
+static const struct iio_chan_spec ak8974_12_bits_channels[] = {
+	AK8974_AXIS_CHANNEL(X, 0, 12),
+	AK8974_AXIS_CHANNEL(Y, 1, 12),
+	AK8974_AXIS_CHANNEL(Z, 2, 12),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+/*
+ * The HSCDTD008A has 15 bits resolution the way we set it up
+ * in CTRL4.
+ */
+static const struct iio_chan_spec ak8974_15_bits_channels[] = {
+	AK8974_AXIS_CHANNEL(X, 0, 15),
+	AK8974_AXIS_CHANNEL(Y, 1, 15),
+	AK8974_AXIS_CHANNEL(Z, 2, 15),
 	IIO_CHAN_SOFT_TIMESTAMP(3),
 };
 
@@ -674,18 +767,18 @@ static bool ak8974_writeable_reg(struct device *dev, unsigned int reg)
 	case AK8974_INT_CTRL:
 	case AK8974_INT_THRES:
 	case AK8974_INT_THRES + 1:
+		return true;
 	case AK8974_PRESET:
 	case AK8974_PRESET + 1:
-		return true;
+		return ak8974->variant != AK8974_WHOAMI_VALUE_HSCDTD008A;
 	case AK8974_OFFSET_X:
 	case AK8974_OFFSET_X + 1:
 	case AK8974_OFFSET_Y:
 	case AK8974_OFFSET_Y + 1:
 	case AK8974_OFFSET_Z:
 	case AK8974_OFFSET_Z + 1:
-		if (ak8974->variant == AK8974_WHOAMI_VALUE_AK8974)
-			return true;
-		return false;
+		return ak8974->variant == AK8974_WHOAMI_VALUE_AK8974 ||
+		       ak8974->variant == AK8974_WHOAMI_VALUE_HSCDTD008A;
 	case AMI305_OFFSET_X:
 	case AMI305_OFFSET_X + 1:
 	case AMI305_OFFSET_Y:
@@ -746,7 +839,12 @@ static int ak8974_probe(struct i2c_client *i2c,
 				      ARRAY_SIZE(ak8974->regs),
 				      ak8974->regs);
 	if (ret < 0) {
-		dev_err(&i2c->dev, "cannot get regulators\n");
+		if (ret != -EPROBE_DEFER)
+			dev_err(&i2c->dev, "cannot get regulators: %d\n", ret);
+		else
+			dev_dbg(&i2c->dev,
+				"regulators unavailable, deferring probe\n");
+
 		return ret;
 	}
 
@@ -795,8 +893,21 @@ static int ak8974_probe(struct i2c_client *i2c,
 	pm_runtime_put(&i2c->dev);
 
 	indio_dev->dev.parent = &i2c->dev;
-	indio_dev->channels = ak8974_channels;
-	indio_dev->num_channels = ARRAY_SIZE(ak8974_channels);
+	switch (ak8974->variant) {
+	case AK8974_WHOAMI_VALUE_AMI306:
+	case AK8974_WHOAMI_VALUE_AMI305:
+		indio_dev->channels = ak8974_12_bits_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ak8974_12_bits_channels);
+		break;
+	case AK8974_WHOAMI_VALUE_HSCDTD008A:
+		indio_dev->channels = ak8974_15_bits_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ak8974_15_bits_channels);
+		break;
+	default:
+		indio_dev->channels = ak8974_12_bits_channels;
+		indio_dev->num_channels = ARRAY_SIZE(ak8974_12_bits_channels);
+		break;
+	}
 	indio_dev->info = &ak8974_info;
 	indio_dev->available_scan_masks = ak8974_scan_masks;
 	indio_dev->modes = INDIO_DIRECT_MODE;
@@ -926,12 +1037,14 @@ static const struct i2c_device_id ak8974_id[] = {
 	{"ami305", 0 },
 	{"ami306", 0 },
 	{"ak8974", 0 },
+	{"hscdtd008a", 0 },
 	{}
 };
 MODULE_DEVICE_TABLE(i2c, ak8974_id);
 
 static const struct of_device_id ak8974_of_match[] = {
 	{ .compatible = "asahi-kasei,ak8974", },
+	{ .compatible = "alps,hscdtd008a", },
 	{}
 };
 MODULE_DEVICE_TABLE(of, ak8974_of_match);
diff --git a/drivers/iio/magnetometer/bmc150_magn_spi.c b/drivers/iio/magnetometer/bmc150_magn_spi.c
index ed9be0490d77..c6ed3ea8460a 100644
--- a/drivers/iio/magnetometer/bmc150_magn_spi.c
+++ b/drivers/iio/magnetometer/bmc150_magn_spi.c
@@ -22,8 +22,8 @@ static int bmc150_magn_spi_probe(struct spi_device *spi)
 
 	regmap = devm_regmap_init_spi(spi, &bmc150_magn_regmap_config);
 	if (IS_ERR(regmap)) {
-		dev_err(&spi->dev, "Failed to register spi regmap %d\n",
-			(int)PTR_ERR(regmap));
+		dev_err(&spi->dev, "Failed to register spi regmap: %pe\n",
+			regmap);
 		return PTR_ERR(regmap);
 	}
 	return bmc150_magn_probe(&spi->dev, regmap, spi->irq, id->name);
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 25e60b233e08..0c09daf87794 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 enum magn_3d_channel {
@@ -519,18 +517,13 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		return ret;
-	}
 	atomic_set(&magn_state->magn_flux_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&magn_state->magn_flux_attributes);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		return ret;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -554,9 +547,7 @@ static int hid_magn_3d_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&magn_state->magn_flux_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &magn_state->magn_flux_attributes);
 	return ret;
 }
 
@@ -569,8 +560,7 @@ static int hid_magn_3d_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_COMPASS_3D);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&magn_state->magn_flux_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &magn_state->magn_flux_attributes);
 
 	return 0;
 }
diff --git a/drivers/iio/magnetometer/mmc35240.c b/drivers/iio/magnetometer/mmc35240.c
index 425cdd07b4e5..1787d656d009 100644
--- a/drivers/iio/magnetometer/mmc35240.c
+++ b/drivers/iio/magnetometer/mmc35240.c
@@ -239,7 +239,7 @@ static int mmc35240_init(struct mmc35240_data *data)
 		return ret;
 
 	ret = regmap_bulk_read(data->regmap, MMC35240_OTP_START_ADDR,
-			       (u8 *)otp_data, sizeof(otp_data));
+			       otp_data, sizeof(otp_data));
 	if (ret < 0)
 		return ret;
 
@@ -295,7 +295,7 @@ static int mmc35240_read_measurement(struct mmc35240_data *data, __le16 buf[3])
 	if (ret < 0)
 		return ret;
 
-	return regmap_bulk_read(data->regmap, MMC35240_REG_XOUT_L, (u8 *)buf,
+	return regmap_bulk_read(data->regmap, MMC35240_REG_XOUT_L, buf,
 				3 * sizeof(__le16));
 }
 
diff --git a/drivers/iio/magnetometer/rm3100-core.c b/drivers/iio/magnetometer/rm3100-core.c
index 7c20918d8108..43a2e420c9c4 100644
--- a/drivers/iio/magnetometer/rm3100-core.c
+++ b/drivers/iio/magnetometer/rm3100-core.c
@@ -22,6 +22,8 @@
 #include <linux/iio/triggered_buffer.h>
 #include <linux/iio/trigger_consumer.h>
 
+#include <asm/unaligned.h>
+
 #include "rm3100.h"
 
 /* Cycle Count Registers. */
@@ -223,8 +225,7 @@ static int rm3100_read_mag(struct rm3100_data *data, int idx, int *val)
 		goto unlock_return;
 	mutex_unlock(&data->lock);
 
-	*val = sign_extend32((buffer[0] << 16) | (buffer[1] << 8) | buffer[2],
-			     23);
+	*val = sign_extend32(get_unaligned_be24(&buffer[0]), 23);
 
 	return IIO_VAL_INT;
 
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index e68184a93a6d..79de721e6015 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -506,8 +506,7 @@ int st_magn_common_probe(struct iio_dev *indio_dev)
 	indio_dev->channels = mdata->sensor_settings->ch;
 	indio_dev->num_channels = ST_SENSORS_NUMBER_ALL_CHANNELS;
 
-	mdata->current_fullscale = (struct st_sensor_fullscale_avl *)
-					&mdata->sensor_settings->fs.fs_avl[0];
+	mdata->current_fullscale = &mdata->sensor_settings->fs.fs_avl[0];
 	mdata->odr = mdata->sensor_settings->odr.odr_avl[0].hz;
 
 	err = st_sensors_init_sensor(indio_dev, NULL);
diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 00af68764cda..6aac8bea233a 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -15,8 +15,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 enum incl_3d_channel {
@@ -346,18 +344,13 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&incl_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&incl_state->common_attributes);
 	if (ret) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -382,9 +375,7 @@ static int hid_incl_3d_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&incl_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &incl_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -399,8 +390,7 @@ static int hid_incl_3d_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_INCLINOMETER_3D);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&incl_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &incl_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
index 64ae7d04a200..b99f41240e3e 100644
--- a/drivers/iio/orientation/hid-sensor-rotation.c
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 struct dev_rot_state {
@@ -288,18 +286,13 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		return ret;
-	}
 	atomic_set(&rot_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 					&rot_state->common_attributes);
 	if (ret) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		return ret;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -323,9 +316,7 @@ static int hid_dev_rot_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&rot_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &rot_state->common_attributes);
 	return ret;
 }
 
@@ -338,8 +329,7 @@ static int hid_dev_rot_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, hsdev->usage);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&rot_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &rot_state->common_attributes);
 
 	return 0;
 }
diff --git a/drivers/iio/pressure/bmp280-core.c b/drivers/iio/pressure/bmp280-core.c
index 29c209cc1108..126a56d31b6e 100644
--- a/drivers/iio/pressure/bmp280-core.c
+++ b/drivers/iio/pressure/bmp280-core.c
@@ -271,6 +271,8 @@ static u32 bmp280_compensate_humidity(struct bmp280_data *data,
 		+ (s32)2097152) * calib->H2 + 8192) >> 14);
 	var -= ((((var >> 15) * (var >> 15)) >> 7) * (s32)calib->H1) >> 4;
 
+	var = clamp_val(var, 0, 419430400);
+
 	return var >> 12;
 };
 
@@ -337,8 +339,7 @@ static int bmp280_read_temp(struct bmp280_data *data,
 	__be32 tmp = 0;
 	s32 adc_temp, comp_temp;
 
-	ret = regmap_bulk_read(data->regmap, BMP280_REG_TEMP_MSB,
-			       (u8 *) &tmp, 3);
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_TEMP_MSB, &tmp, 3);
 	if (ret < 0) {
 		dev_err(data->dev, "failed to read temperature\n");
 		return ret;
@@ -377,8 +378,7 @@ static int bmp280_read_press(struct bmp280_data *data,
 	if (ret < 0)
 		return ret;
 
-	ret = regmap_bulk_read(data->regmap, BMP280_REG_PRESS_MSB,
-			       (u8 *) &tmp, 3);
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_PRESS_MSB, &tmp, 3);
 	if (ret < 0) {
 		dev_err(data->dev, "failed to read pressure\n");
 		return ret;
@@ -400,8 +400,8 @@ static int bmp280_read_press(struct bmp280_data *data,
 
 static int bmp280_read_humid(struct bmp280_data *data, int *val, int *val2)
 {
+	__be16 tmp;
 	int ret;
-	__be16 tmp = 0;
 	s32 adc_humidity;
 	u32 comp_humidity;
 
@@ -410,8 +410,7 @@ static int bmp280_read_humid(struct bmp280_data *data, int *val, int *val2)
 	if (ret < 0)
 		return ret;
 
-	ret = regmap_bulk_read(data->regmap, BMP280_REG_HUMIDITY_MSB,
-			       (u8 *) &tmp, 2);
+	ret = regmap_bulk_read(data->regmap, BMP280_REG_HUMIDITY_MSB, &tmp, 2);
 	if (ret < 0) {
 		dev_err(data->dev, "failed to read humidity\n");
 		return ret;
@@ -575,57 +574,38 @@ static int bmp280_write_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
-static ssize_t bmp280_show_avail(char *buf, const int *vals, const int n)
+static int bmp280_read_avail(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     const int **vals, int *type, int *length,
+			     long mask)
 {
-	size_t len = 0;
-	int i;
-
-	for (i = 0; i < n; i++)
-		len += scnprintf(buf + len, PAGE_SIZE - len, "%d ", vals[i]);
-
-	buf[len - 1] = '\n';
-
-	return len;
-}
-
-static ssize_t bmp280_show_temp_oversampling_avail(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct bmp280_data *data = iio_priv(dev_to_iio_dev(dev));
-
-	return bmp280_show_avail(buf, data->chip_info->oversampling_temp_avail,
-				 data->chip_info->num_oversampling_temp_avail);
-}
-
-static ssize_t bmp280_show_press_oversampling_avail(struct device *dev,
-				struct device_attribute *attr, char *buf)
-{
-	struct bmp280_data *data = iio_priv(dev_to_iio_dev(dev));
+	struct bmp280_data *data = iio_priv(indio_dev);
 
-	return bmp280_show_avail(buf, data->chip_info->oversampling_press_avail,
-				 data->chip_info->num_oversampling_press_avail);
+	switch (mask) {
+	case IIO_CHAN_INFO_OVERSAMPLING_RATIO:
+		switch (chan->type) {
+		case IIO_PRESSURE:
+			*vals = data->chip_info->oversampling_press_avail;
+			*length = data->chip_info->num_oversampling_press_avail;
+			break;
+		case IIO_TEMP:
+			*vals = data->chip_info->oversampling_temp_avail;
+			*length = data->chip_info->num_oversampling_temp_avail;
+			break;
+		default:
+			return -EINVAL;
+		}
+		*type = IIO_VAL_INT;
+		return IIO_AVAIL_LIST;
+	default:
+		return -EINVAL;
+	}
 }
 
-static IIO_DEVICE_ATTR(in_temp_oversampling_ratio_available,
-	S_IRUGO, bmp280_show_temp_oversampling_avail, NULL, 0);
-
-static IIO_DEVICE_ATTR(in_pressure_oversampling_ratio_available,
-	S_IRUGO, bmp280_show_press_oversampling_avail, NULL, 0);
-
-static struct attribute *bmp280_attributes[] = {
-	&iio_dev_attr_in_temp_oversampling_ratio_available.dev_attr.attr,
-	&iio_dev_attr_in_pressure_oversampling_ratio_available.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group bmp280_attrs_group = {
-	.attrs = bmp280_attributes,
-};
-
 static const struct iio_info bmp280_info = {
 	.read_raw = &bmp280_read_raw,
+	.read_avail = &bmp280_read_avail,
 	.write_raw = &bmp280_write_raw,
-	.attrs = &bmp280_attrs_group,
 };
 
 static int bmp280_chip_config(struct bmp280_data *data)
@@ -713,7 +693,7 @@ static int bmp180_measure(struct bmp280_data *data, u8 ctrl_meas)
 	unsigned int ctrl;
 
 	if (data->use_eoc)
-		init_completion(&data->done);
+		reinit_completion(&data->done);
 
 	ret = regmap_write(data->regmap, BMP280_REG_CTRL_MEAS, ctrl_meas);
 	if (ret)
@@ -752,14 +732,14 @@ static int bmp180_measure(struct bmp280_data *data, u8 ctrl_meas)
 
 static int bmp180_read_adc_temp(struct bmp280_data *data, int *val)
 {
+	__be16 tmp;
 	int ret;
-	__be16 tmp = 0;
 
 	ret = bmp180_measure(data, BMP180_MEAS_TEMP);
 	if (ret)
 		return ret;
 
-	ret = regmap_bulk_read(data->regmap, BMP180_REG_OUT_MSB, (u8 *)&tmp, 2);
+	ret = regmap_bulk_read(data->regmap, BMP180_REG_OUT_MSB, &tmp, 2);
 	if (ret)
 		return ret;
 
@@ -856,7 +836,7 @@ static int bmp180_read_adc_press(struct bmp280_data *data, int *val)
 	if (ret)
 		return ret;
 
-	ret = regmap_bulk_read(data->regmap, BMP180_REG_OUT_MSB, (u8 *)&tmp, 3);
+	ret = regmap_bulk_read(data->regmap, BMP180_REG_OUT_MSB, &tmp, 3);
 	if (ret)
 		return ret;
 
@@ -965,10 +945,12 @@ static int bmp085_fetch_eoc_irq(struct device *dev,
 
 	irq_trig = irqd_get_trigger_type(irq_get_irq_data(irq));
 	if (irq_trig != IRQF_TRIGGER_RISING) {
-		dev_err(dev, "non-rising trigger given for EOC interrupt, "
-			"trying to enforce it\n");
+		dev_err(dev, "non-rising trigger given for EOC interrupt, trying to enforce it\n");
 		irq_trig = IRQF_TRIGGER_RISING;
 	}
+
+	init_completion(&data->done);
+
 	ret = devm_request_threaded_irq(dev,
 			irq,
 			bmp085_eoc_irq,
@@ -1082,9 +1064,9 @@ int bmp280_common_probe(struct device *dev,
 	usleep_range(data->start_up_time, data->start_up_time + 100);
 
 	/* Bring chip out of reset if there is an assigned GPIO line */
-	gpiod = devm_gpiod_get(dev, "reset", GPIOD_OUT_HIGH);
+	gpiod = devm_gpiod_get_optional(dev, "reset", GPIOD_OUT_HIGH);
 	/* Deassert the signal */
-	if (!IS_ERR(gpiod)) {
+	if (gpiod) {
 		dev_info(dev, "release reset\n");
 		gpiod_set_value(gpiod, 0);
 	}
diff --git a/drivers/iio/pressure/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index 953235052155..5e6663f757ae 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -14,8 +14,6 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
-#include <linux/iio/trigger_consumer.h>
-#include <linux/iio/triggered_buffer.h>
 #include "../common/hid-sensors/hid-sensor-trigger.h"
 
 #define CHANNEL_SCAN_INDEX_PRESSURE 0
@@ -290,18 +288,13 @@ static int hid_press_probe(struct platform_device *pdev)
 	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 
-	ret = iio_triggered_buffer_setup(indio_dev, &iio_pollfunc_store_time,
-		NULL, NULL);
-	if (ret) {
-		dev_err(&pdev->dev, "failed to initialize trigger buffer\n");
-		goto error_free_dev_mem;
-	}
 	atomic_set(&press_state->common_attributes.data_ready, 0);
+
 	ret = hid_sensor_setup_trigger(indio_dev, name,
 				&press_state->common_attributes);
 	if (ret) {
 		dev_err(&pdev->dev, "trigger setup failed\n");
-		goto error_unreg_buffer_funcs;
+		goto error_free_dev_mem;
 	}
 
 	ret = iio_device_register(indio_dev);
@@ -325,9 +318,7 @@ static int hid_press_probe(struct platform_device *pdev)
 error_iio_unreg:
 	iio_device_unregister(indio_dev);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&press_state->common_attributes);
-error_unreg_buffer_funcs:
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &press_state->common_attributes);
 error_free_dev_mem:
 	kfree(indio_dev->channels);
 	return ret;
@@ -342,8 +333,7 @@ static int hid_press_remove(struct platform_device *pdev)
 
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_PRESSURE);
 	iio_device_unregister(indio_dev);
-	hid_sensor_remove_trigger(&press_state->common_attributes);
-	iio_triggered_buffer_cleanup(indio_dev);
+	hid_sensor_remove_trigger(indio_dev, &press_state->common_attributes);
 	kfree(indio_dev->channels);
 
 	return 0;
diff --git a/drivers/iio/pressure/hp206c.c b/drivers/iio/pressure/hp206c.c
index 3ac3632e7242..1f931f5b7a65 100644
--- a/drivers/iio/pressure/hp206c.c
+++ b/drivers/iio/pressure/hp206c.c
@@ -18,6 +18,8 @@
 #include <linux/util_macros.h>
 #include <linux/acpi.h>
 
+#include <asm/unaligned.h>
+
 /* I2C commands: */
 #define HP206C_CMD_SOFT_RST	0x06
 
@@ -93,12 +95,12 @@ static int hp206c_read_20bit(struct i2c_client *client, u8 cmd)
 	int ret;
 	u8 values[3];
 
-	ret = i2c_smbus_read_i2c_block_data(client, cmd, 3, values);
+	ret = i2c_smbus_read_i2c_block_data(client, cmd, sizeof(values), values);
 	if (ret < 0)
 		return ret;
-	if (ret != 3)
+	if (ret != sizeof(values))
 		return -EIO;
-	return ((values[0] & 0xF) << 16) | (values[1] << 8) | (values[2]);
+	return get_unaligned_be24(&values[0]) & GENMASK(19, 0);
 }
 
 /* Spin for max 160ms until DEV_RDY is 1, or return error. */
diff --git a/drivers/iio/pressure/ms5611_i2c.c b/drivers/iio/pressure/ms5611_i2c.c
index 8089c59adce5..072c106dd66d 100644
--- a/drivers/iio/pressure/ms5611_i2c.c
+++ b/drivers/iio/pressure/ms5611_i2c.c
@@ -16,6 +16,8 @@
 #include <linux/module.h>
 #include <linux/of_device.h>
 
+#include <asm/unaligned.h>
+
 #include "ms5611.h"
 
 static int ms5611_i2c_reset(struct device *dev)
@@ -50,7 +52,7 @@ static int ms5611_i2c_read_adc(struct ms5611_state *st, s32 *val)
 	if (ret < 0)
 		return ret;
 
-	*val = (buf[0] << 16) | (buf[1] << 8) | buf[2];
+	*val = get_unaligned_be24(&buf[0]);
 
 	return 0;
 }
diff --git a/drivers/iio/pressure/ms5611_spi.c b/drivers/iio/pressure/ms5611_spi.c
index b463eaa799ab..4799aa57135e 100644
--- a/drivers/iio/pressure/ms5611_spi.c
+++ b/drivers/iio/pressure/ms5611_spi.c
@@ -11,6 +11,8 @@
 #include <linux/spi/spi.h>
 #include <linux/of_device.h>
 
+#include <asm/unaligned.h>
+
 #include "ms5611.h"
 
 static int ms5611_spi_reset(struct device *dev)
@@ -45,7 +47,7 @@ static int ms5611_spi_read_adc(struct device *dev, s32 *val)
 	if (ret < 0)
 		return ret;
 
-	*val = (buf[0] << 16) | (buf[1] << 8) | buf[2];
+	*val = get_unaligned_be24(&buf[0]);
 
 	return 0;
 }
diff --git a/drivers/iio/pressure/st_pressure_core.c b/drivers/iio/pressure/st_pressure_core.c
index bd972cec4830..789a2928504a 100644
--- a/drivers/iio/pressure/st_pressure_core.c
+++ b/drivers/iio/pressure/st_pressure_core.c
@@ -683,8 +683,7 @@ EXPORT_SYMBOL(st_press_get_settings);
 int st_press_common_probe(struct iio_dev *indio_dev)
 {
 	struct st_sensor_data *press_data = iio_priv(indio_dev);
-	struct st_sensors_platform_data *pdata =
-		(struct st_sensors_platform_data *)press_data->dev->platform_data;
+	struct st_sensors_platform_data *pdata = dev_get_platdata(press_data->dev);
 	int err;
 
 	indio_dev->modes = INDIO_DIRECT_MODE;
@@ -708,9 +707,7 @@ int st_press_common_probe(struct iio_dev *indio_dev)
 	indio_dev->channels = press_data->sensor_settings->ch;
 	indio_dev->num_channels = press_data->sensor_settings->num_ch;
 
-	press_data->current_fullscale =
-		(struct st_sensor_fullscale_avl *)
-			&press_data->sensor_settings->fs.fs_avl[0];
+	press_data->current_fullscale = &press_data->sensor_settings->fs.fs_avl[0];
 
 	press_data->odr = press_data->sensor_settings->odr.odr_avl[0].hz;
 
diff --git a/drivers/iio/pressure/zpa2326.c b/drivers/iio/pressure/zpa2326.c
index 99dfe33ee402..37fe851f89af 100644
--- a/drivers/iio/pressure/zpa2326.c
+++ b/drivers/iio/pressure/zpa2326.c
@@ -64,6 +64,7 @@
 #include <linux/iio/trigger.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
+#include <asm/unaligned.h>
 #include "zpa2326.h"
 
 /* 200 ms should be enough for the longest conversion time in one-shot mode. */
@@ -1005,22 +1006,20 @@ static int zpa2326_fetch_raw_sample(const struct iio_dev *indio_dev,
 	struct regmap *regs = ((struct zpa2326_private *)
 			       iio_priv(indio_dev))->regmap;
 	int            err;
+	u8             v[3];
 
 	switch (type) {
 	case IIO_PRESSURE:
 		zpa2326_dbg(indio_dev, "fetching raw pressure sample");
 
-		err = regmap_bulk_read(regs, ZPA2326_PRESS_OUT_XL_REG, value,
-				       3);
+		err = regmap_bulk_read(regs, ZPA2326_PRESS_OUT_XL_REG, v, sizeof(v));
 		if (err) {
 			zpa2326_warn(indio_dev, "failed to fetch pressure (%d)",
 				     err);
 			return err;
 		}
 
-		/* Pressure is a 24 bits wide little-endian unsigned int. */
-		*value = (((u8 *)value)[2] << 16) | (((u8 *)value)[1] << 8) |
-			 ((u8 *)value)[0];
+		*value = get_unaligned_le24(&v[0]);
 
 		return IIO_VAL_INT;
 
diff --git a/drivers/iio/proximity/Kconfig b/drivers/iio/proximity/Kconfig
index 37606d400805..12672a0e89ed 100644
--- a/drivers/iio/proximity/Kconfig
+++ b/drivers/iio/proximity/Kconfig
@@ -101,6 +101,19 @@ config SRF04
 	  To compile this driver as a module, choose M here: the
 	  module will be called srf04.
 
+config SX9310
+	tristate "SX9310/SX9311 Semtech proximity sensor"
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	select REGMAP_I2C
+	depends on I2C
+	help
+	  Say Y here to build a driver for Semtech's SX9310/SX9311 capacitive
+	  proximity/button sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called sx9310.
+
 config SX9500
 	tristate "SX9500 Semtech proximity sensor"
 	select IIO_BUFFER
@@ -127,6 +140,17 @@ config SRF08
 	  To compile this driver as a module, choose M here: the
 	  module will be called srf08.
 
+config VCNL3020
+	tristate "VCNL3020 proximity sensor"
+	select REGMAP_I2C
+	depends on I2C
+	help
+	  Say Y here if you want to build a driver for the Vishay VCNL3020
+	  proximity sensor.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called vcnl3020.
+
 config VL53L0X_I2C
 	tristate "STMicroelectronics VL53L0X ToF ranger sensor (I2C)"
 	depends on I2C
diff --git a/drivers/iio/proximity/Makefile b/drivers/iio/proximity/Makefile
index c591b019304e..9c1aca1a8b79 100644
--- a/drivers/iio/proximity/Makefile
+++ b/drivers/iio/proximity/Makefile
@@ -12,6 +12,8 @@ obj-$(CONFIG_PING)		+= ping.o
 obj-$(CONFIG_RFD77402)		+= rfd77402.o
 obj-$(CONFIG_SRF04)		+= srf04.o
 obj-$(CONFIG_SRF08)		+= srf08.o
+obj-$(CONFIG_SX9310)		+= sx9310.o
 obj-$(CONFIG_SX9500)		+= sx9500.o
+obj-$(CONFIG_VCNL3020)		+= vcnl3020.o
 obj-$(CONFIG_VL53L0X_I2C)	+= vl53l0x-i2c.o
 
diff --git a/drivers/iio/proximity/ping.c b/drivers/iio/proximity/ping.c
index 12b893c5b0ee..2e99eeb27f2e 100644
--- a/drivers/iio/proximity/ping.c
+++ b/drivers/iio/proximity/ping.c
@@ -89,14 +89,14 @@ static irqreturn_t ping_handle_irq(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int ping_read(struct ping_data *data)
+static int ping_read(struct iio_dev *indio_dev)
 {
+	struct ping_data *data = iio_priv(indio_dev);
 	int ret;
 	ktime_t ktime_dt;
 	s64 dt_ns;
 	u32 time_ns, distance_mm;
 	struct platform_device *pdev = to_platform_device(data->dev);
-	struct iio_dev *indio_dev = iio_priv_to_dev(data);
 
 	/*
 	 * just one read-echo-cycle can take place at a time
@@ -228,7 +228,6 @@ static int ping_read_raw(struct iio_dev *indio_dev,
 			    struct iio_chan_spec const *channel, int *val,
 			    int *val2, long info)
 {
-	struct ping_data *data = iio_priv(indio_dev);
 	int ret;
 
 	if (channel->type != IIO_DISTANCE)
@@ -236,7 +235,7 @@ static int ping_read_raw(struct iio_dev *indio_dev,
 
 	switch (info) {
 	case IIO_CHAN_INFO_RAW:
-		ret = ping_read(data);
+		ret = ping_read(indio_dev);
 		if (ret < 0)
 			return ret;
 		*val = ret;
diff --git a/drivers/iio/proximity/sx9310.c b/drivers/iio/proximity/sx9310.c
new file mode 100644
index 000000000000..d161f3061e35
--- /dev/null
+++ b/drivers/iio/proximity/sx9310.c
@@ -0,0 +1,1069 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright 2018 Google LLC.
+ *
+ * Driver for Semtech's SX9310/SX9311 capacitive proximity/button solution.
+ * Based on SX9500 driver and Semtech driver using the input framework
+ * <https://my.syncplicity.com/share/teouwsim8niiaud/
+ *          linux-driver-SX9310_NoSmartHSensing>.
+ * Reworked April 2019 by Evan Green <evgreen@chromium.org>
+ * and January 2020 by Daniel Campello <campello@chromium.org>
+ */
+
+#include <linux/acpi.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/triggered_buffer.h>
+#include <linux/iio/trigger_consumer.h>
+
+/* Register definitions. */
+#define SX9310_REG_IRQ_SRC				0x00
+#define SX9310_REG_STAT0				0x01
+#define SX9310_REG_STAT1				0x02
+#define SX9310_REG_IRQ_MSK				0x03
+#define   SX9310_CONVDONE_IRQ				BIT(3)
+#define   SX9310_FAR_IRQ				BIT(5)
+#define   SX9310_CLOSE_IRQ				BIT(6)
+#define   SX9310_EVENT_IRQ				(SX9310_FAR_IRQ | \
+							 SX9310_CLOSE_IRQ)
+#define SX9310_REG_IRQ_FUNC				0x04
+
+#define SX9310_REG_PROX_CTRL0				0x10
+#define   SX9310_REG_PROX_CTRL0_PROXSTAT2		0x10
+#define   SX9310_REG_PROX_CTRL0_EN_MASK			0x0F
+#define SX9310_REG_PROX_CTRL1				0x11
+#define SX9310_REG_PROX_CTRL2				0x12
+#define   SX9310_REG_PROX_CTRL2_COMBMODE_ALL		0x80
+#define   SX9310_REG_PROX_CTRL2_SHIELDEN_DYNAMIC	0x04
+#define SX9310_REG_PROX_CTRL3				0x13
+#define   SX9310_REG_PROX_CTRL3_GAIN0_X8		0x0c
+#define   SX9310_REG_PROX_CTRL3_GAIN12_X4		0x02
+#define SX9310_REG_PROX_CTRL4				0x14
+#define   SX9310_REG_PROX_CTRL4_RESOLUTION_FINEST	0x07
+#define SX9310_REG_PROX_CTRL5				0x15
+#define   SX9310_REG_PROX_CTRL5_RANGE_SMALL		0xc0
+#define   SX9310_REG_PROX_CTRL5_STARTUPSENS_CS1		0x04
+#define   SX9310_REG_PROX_CTRL5_RAWFILT_1P25		0x02
+#define SX9310_REG_PROX_CTRL6				0x16
+#define   SX9310_REG_PROX_CTRL6_COMP_COMMON		0x20
+#define SX9310_REG_PROX_CTRL7				0x17
+#define   SX9310_REG_PROX_CTRL7_AVGNEGFILT_2		0x08
+#define   SX9310_REG_PROX_CTRL7_AVGPOSFILT_512		0x05
+#define SX9310_REG_PROX_CTRL8				0x18
+#define SX9310_REG_PROX_CTRL9				0x19
+#define   SX9310_REG_PROX_CTRL8_9_PTHRESH12_28		0x40
+#define   SX9310_REG_PROX_CTRL8_9_PTHRESH_96		0x88
+#define   SX9310_REG_PROX_CTRL8_9_BODYTHRESH_900	0x03
+#define   SX9310_REG_PROX_CTRL8_9_BODYTHRESH_1500	0x05
+#define SX9310_REG_PROX_CTRL10				0x1a
+#define   SX9310_REG_PROX_CTRL10_HYST_6PCT		0x10
+#define   SX9310_REG_PROX_CTRL10_CLOSE_DEBOUNCE_8	0x12
+#define   SX9310_REG_PROX_CTRL10_FAR_DEBOUNCE_8		0x03
+#define SX9310_REG_PROX_CTRL11				0x1b
+#define SX9310_REG_PROX_CTRL12				0x1c
+#define SX9310_REG_PROX_CTRL13				0x1d
+#define SX9310_REG_PROX_CTRL14				0x1e
+#define SX9310_REG_PROX_CTRL15				0x1f
+#define SX9310_REG_PROX_CTRL16				0x20
+#define SX9310_REG_PROX_CTRL17				0x21
+#define SX9310_REG_PROX_CTRL18				0x22
+#define SX9310_REG_PROX_CTRL19				0x23
+#define SX9310_REG_SAR_CTRL0				0x2a
+#define   SX9310_REG_SAR_CTRL0_SARDEB_4_SAMPLES		0x40
+#define   SX9310_REG_SAR_CTRL0_SARHYST_8		0x10
+#define SX9310_REG_SAR_CTRL1				0x2b
+/* Each increment of the slope register is 0.0078125. */
+#define   SX9310_REG_SAR_CTRL1_SLOPE(_hnslope)		(_hnslope / 78125)
+#define SX9310_REG_SAR_CTRL2				0x2c
+#define   SX9310_REG_SAR_CTRL2_SAROFFSET_DEFAULT	0x3c
+
+#define SX9310_REG_SENSOR_SEL				0x30
+
+#define SX9310_REG_USE_MSB				0x31
+#define SX9310_REG_USE_LSB				0x32
+
+#define SX9310_REG_AVG_MSB				0x33
+#define SX9310_REG_AVG_LSB				0x34
+
+#define SX9310_REG_DIFF_MSB				0x35
+#define SX9310_REG_DIFF_LSB				0x36
+
+#define SX9310_REG_OFFSET_MSB				0x37
+#define SX9310_REG_OFFSET_LSB				0x38
+
+#define SX9310_REG_SAR_MSB				0x39
+#define SX9310_REG_SAR_LSB				0x3a
+
+#define SX9310_REG_I2CADDR				0x40
+#define SX9310_REG_PAUSE				0x41
+#define SX9310_REG_WHOAMI				0x42
+#define   SX9310_WHOAMI_VALUE				0x01
+#define   SX9311_WHOAMI_VALUE				0x02
+
+#define SX9310_REG_RESET				0x7f
+#define   SX9310_SOFT_RESET				0xde
+
+#define SX9310_SCAN_PERIOD_MASK				GENMASK(7, 4)
+#define SX9310_SCAN_PERIOD_SHIFT			4
+
+#define SX9310_COMPSTAT_MASK				GENMASK(3, 0)
+
+/* 4 hardware channels, as defined in STAT0: COMB, CS2, CS1 and CS0. */
+#define SX9310_NUM_CHANNELS				4
+#define SX9310_CHAN_ENABLED_MASK			GENMASK(3, 0)
+
+struct sx9310_data {
+	/* Serialize access to registers and channel configuration */
+	struct mutex mutex;
+	struct i2c_client *client;
+	struct iio_trigger *trig;
+	struct regmap *regmap;
+	/*
+	 * Last reading of the proximity status for each channel.
+	 * We only send an event to user space when this changes.
+	 */
+	bool prox_stat[SX9310_NUM_CHANNELS];
+	bool trigger_enabled;
+	__be16 buffer[SX9310_NUM_CHANNELS +
+		      4]; /* 64-bit data + 64-bit timestamp */
+	/* Remember enabled channels and sample rate during suspend. */
+	unsigned int suspend_ctrl0;
+	struct completion completion;
+	unsigned int chan_read, chan_event;
+	int channel_users[SX9310_NUM_CHANNELS];
+	int whoami;
+};
+
+static const struct iio_event_spec sx9310_events[] = {
+	{
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_EITHER,
+		.mask_separate = BIT(IIO_EV_INFO_ENABLE),
+	},
+};
+
+#define SX9310_NAMED_CHANNEL(idx, name)					 \
+	{								 \
+		.type = IIO_PROXIMITY,					 \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		 \
+		.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SAMP_FREQ), \
+		.indexed = 1,						 \
+		.channel = idx,						 \
+		.extend_name = name,					 \
+		.address = SX9310_REG_DIFF_MSB,				 \
+		.event_spec = sx9310_events,				 \
+		.num_event_specs = ARRAY_SIZE(sx9310_events),		 \
+		.scan_index = idx,					 \
+		.scan_type = {						 \
+			.sign = 's',					 \
+			.realbits = 12,					 \
+			.storagebits = 16,				 \
+			.endianness = IIO_BE,				 \
+		},							 \
+	}
+#define SX9310_CHANNEL(idx) SX9310_NAMED_CHANNEL(idx, NULL)
+
+static const struct iio_chan_spec sx9310_channels[] = {
+	SX9310_CHANNEL(0),			/* CS0 */
+	SX9310_CHANNEL(1),			/* CS1 */
+	SX9310_CHANNEL(2),			/* CS2 */
+	SX9310_NAMED_CHANNEL(3, "comb"),	/* COMB */
+
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+/*
+ * Each entry contains the integer part (val) and the fractional part, in micro
+ * seconds. It conforms to the IIO output IIO_VAL_INT_PLUS_MICRO.
+ */
+static const struct {
+	int val;
+	int val2;
+} sx9310_samp_freq_table[] = {
+	{ 500, 0 }, /* 0000: Min (no idle time) */
+	{ 66, 666666 }, /* 0001: 15 ms */
+	{ 33, 333333 }, /* 0010: 30 ms (Typ.) */
+	{ 22, 222222 }, /* 0011: 45 ms */
+	{ 16, 666666 }, /* 0100: 60 ms */
+	{ 11, 111111 }, /* 0101: 90 ms */
+	{ 8, 333333 }, /* 0110: 120 ms */
+	{ 5, 0 }, /* 0111: 200 ms */
+	{ 2, 500000 }, /* 1000: 400 ms */
+	{ 1, 666666 }, /* 1001: 600 ms */
+	{ 1, 250000 }, /* 1010: 800 ms */
+	{ 1, 0 }, /* 1011: 1 s */
+	{ 0, 500000 }, /* 1100: 2 s */
+	{ 0, 333333 }, /* 1101: 3 s */
+	{ 0, 250000 }, /* 1110: 4 s */
+	{ 0, 200000 }, /* 1111: 5 s */
+};
+static const unsigned int sx9310_scan_period_table[] = {
+	2,   15,  30,  45,   60,   90,	 120,  200,
+	400, 600, 800, 1000, 2000, 3000, 4000, 5000,
+};
+
+static ssize_t sx9310_show_samp_freq_avail(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	size_t len = 0;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sx9310_samp_freq_table); i++)
+		len += scnprintf(buf + len, PAGE_SIZE - len, "%d.%d ",
+				 sx9310_samp_freq_table[i].val,
+				 sx9310_samp_freq_table[i].val2);
+	buf[len - 1] = '\n';
+	return len;
+}
+static IIO_DEV_ATTR_SAMP_FREQ_AVAIL(sx9310_show_samp_freq_avail);
+
+static const struct regmap_range sx9310_writable_reg_ranges[] = {
+	regmap_reg_range(SX9310_REG_IRQ_MSK, SX9310_REG_IRQ_FUNC),
+	regmap_reg_range(SX9310_REG_PROX_CTRL0, SX9310_REG_PROX_CTRL19),
+	regmap_reg_range(SX9310_REG_SAR_CTRL0, SX9310_REG_SAR_CTRL2),
+	regmap_reg_range(SX9310_REG_SENSOR_SEL, SX9310_REG_SENSOR_SEL),
+	regmap_reg_range(SX9310_REG_OFFSET_MSB, SX9310_REG_OFFSET_LSB),
+	regmap_reg_range(SX9310_REG_PAUSE, SX9310_REG_PAUSE),
+	regmap_reg_range(SX9310_REG_RESET, SX9310_REG_RESET),
+};
+
+static const struct regmap_access_table sx9310_writeable_regs = {
+	.yes_ranges = sx9310_writable_reg_ranges,
+	.n_yes_ranges = ARRAY_SIZE(sx9310_writable_reg_ranges),
+};
+
+static const struct regmap_range sx9310_readable_reg_ranges[] = {
+	regmap_reg_range(SX9310_REG_IRQ_SRC, SX9310_REG_IRQ_FUNC),
+	regmap_reg_range(SX9310_REG_PROX_CTRL0, SX9310_REG_PROX_CTRL19),
+	regmap_reg_range(SX9310_REG_SAR_CTRL0, SX9310_REG_SAR_CTRL2),
+	regmap_reg_range(SX9310_REG_SENSOR_SEL, SX9310_REG_SAR_LSB),
+	regmap_reg_range(SX9310_REG_I2CADDR, SX9310_REG_WHOAMI),
+	regmap_reg_range(SX9310_REG_RESET, SX9310_REG_RESET),
+};
+
+static const struct regmap_access_table sx9310_readable_regs = {
+	.yes_ranges = sx9310_readable_reg_ranges,
+	.n_yes_ranges = ARRAY_SIZE(sx9310_readable_reg_ranges),
+};
+
+static const struct regmap_range sx9310_volatile_reg_ranges[] = {
+	regmap_reg_range(SX9310_REG_IRQ_SRC, SX9310_REG_STAT1),
+	regmap_reg_range(SX9310_REG_USE_MSB, SX9310_REG_DIFF_LSB),
+	regmap_reg_range(SX9310_REG_SAR_MSB, SX9310_REG_SAR_LSB),
+	regmap_reg_range(SX9310_REG_RESET, SX9310_REG_RESET),
+};
+
+static const struct regmap_access_table sx9310_volatile_regs = {
+	.yes_ranges = sx9310_volatile_reg_ranges,
+	.n_yes_ranges = ARRAY_SIZE(sx9310_volatile_reg_ranges),
+};
+
+static const struct regmap_config sx9310_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = SX9310_REG_RESET,
+	.cache_type = REGCACHE_RBTREE,
+
+	.wr_table = &sx9310_writeable_regs,
+	.rd_table = &sx9310_readable_regs,
+	.volatile_table = &sx9310_volatile_regs,
+};
+
+static int sx9310_update_chan_en(struct sx9310_data *data,
+				 unsigned int chan_read,
+				 unsigned int chan_event)
+{
+	int ret;
+
+	if ((data->chan_read | data->chan_event) != (chan_read | chan_event)) {
+		ret = regmap_update_bits(data->regmap, SX9310_REG_PROX_CTRL0,
+					 SX9310_CHAN_ENABLED_MASK,
+					 chan_read | chan_event);
+		if (ret)
+			return ret;
+	}
+	data->chan_read = chan_read;
+	data->chan_event = chan_event;
+	return 0;
+}
+
+static int sx9310_get_read_channel(struct sx9310_data *data, int channel)
+{
+	return sx9310_update_chan_en(data, data->chan_read | BIT(channel),
+				     data->chan_event);
+}
+
+static int sx9310_put_read_channel(struct sx9310_data *data, int channel)
+{
+	return sx9310_update_chan_en(data, data->chan_read & ~BIT(channel),
+				     data->chan_event);
+}
+
+static int sx9310_get_event_channel(struct sx9310_data *data, int channel)
+{
+	return sx9310_update_chan_en(data, data->chan_read,
+				     data->chan_event | BIT(channel));
+}
+
+static int sx9310_put_event_channel(struct sx9310_data *data, int channel)
+{
+	return sx9310_update_chan_en(data, data->chan_read,
+				     data->chan_event & ~BIT(channel));
+}
+
+static int sx9310_enable_irq(struct sx9310_data *data, unsigned int irq)
+{
+	return regmap_update_bits(data->regmap, SX9310_REG_IRQ_MSK, irq, irq);
+}
+
+static int sx9310_disable_irq(struct sx9310_data *data, unsigned int irq)
+{
+	return regmap_update_bits(data->regmap, SX9310_REG_IRQ_MSK, irq, 0);
+}
+
+static int sx9310_read_prox_data(struct sx9310_data *data,
+				 const struct iio_chan_spec *chan, __be16 *val)
+{
+	int ret;
+
+	ret = regmap_write(data->regmap, SX9310_REG_SENSOR_SEL, chan->channel);
+	if (ret < 0)
+		return ret;
+
+	return regmap_bulk_read(data->regmap, chan->address, val, 2);
+}
+
+/*
+ * If we have no interrupt support, we have to wait for a scan period
+ * after enabling a channel to get a result.
+ */
+static int sx9310_wait_for_sample(struct sx9310_data *data)
+{
+	int ret;
+	unsigned int val;
+
+	ret = regmap_read(data->regmap, SX9310_REG_PROX_CTRL0, &val);
+	if (ret < 0)
+		return ret;
+
+	val = (val & SX9310_SCAN_PERIOD_MASK) >> SX9310_SCAN_PERIOD_SHIFT;
+
+	msleep(sx9310_scan_period_table[val]);
+
+	return 0;
+}
+
+static int sx9310_read_proximity(struct sx9310_data *data,
+				 const struct iio_chan_spec *chan, int *val)
+{
+	int ret = 0;
+	__be16 rawval;
+
+	mutex_lock(&data->mutex);
+
+	ret = sx9310_get_read_channel(data, chan->channel);
+	if (ret < 0)
+		goto out;
+
+	ret = sx9310_enable_irq(data, SX9310_CONVDONE_IRQ);
+	if (ret < 0)
+		goto out_put_channel;
+
+	mutex_unlock(&data->mutex);
+
+	if (data->client->irq > 0) {
+		ret = wait_for_completion_interruptible(&data->completion);
+		reinit_completion(&data->completion);
+	} else {
+		ret = sx9310_wait_for_sample(data);
+	}
+
+	mutex_lock(&data->mutex);
+
+	if (ret < 0)
+		goto out_disable_irq;
+
+	ret = sx9310_read_prox_data(data, chan, &rawval);
+	if (ret < 0)
+		goto out_disable_irq;
+
+	*val = sign_extend32(be16_to_cpu(rawval),
+			     (chan->address == SX9310_REG_DIFF_MSB ? 11 : 15));
+
+	ret = sx9310_disable_irq(data, SX9310_CONVDONE_IRQ);
+	if (ret < 0)
+		goto out_put_channel;
+
+	ret = sx9310_put_read_channel(data, chan->channel);
+	if (ret < 0)
+		goto out;
+
+	mutex_unlock(&data->mutex);
+
+	return IIO_VAL_INT;
+
+out_disable_irq:
+	sx9310_disable_irq(data, SX9310_CONVDONE_IRQ);
+out_put_channel:
+	sx9310_put_read_channel(data, chan->channel);
+out:
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int sx9310_read_samp_freq(struct sx9310_data *data, int *val, int *val2)
+{
+	unsigned int regval;
+	int ret = regmap_read(data->regmap, SX9310_REG_PROX_CTRL0, &regval);
+
+	if (ret < 0)
+		return ret;
+
+	regval = (regval & SX9310_SCAN_PERIOD_MASK) >> SX9310_SCAN_PERIOD_SHIFT;
+	*val = sx9310_samp_freq_table[regval].val;
+	*val2 = sx9310_samp_freq_table[regval].val2;
+
+	return IIO_VAL_INT_PLUS_MICRO;
+}
+
+static int sx9310_read_raw(struct iio_dev *indio_dev,
+			   const struct iio_chan_spec *chan, int *val,
+			   int *val2, long mask)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (chan->type != IIO_PROXIMITY)
+		return -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		ret = iio_device_claim_direct_mode(indio_dev);
+		if (ret)
+			return ret;
+
+		ret = sx9310_read_proximity(data, chan, val);
+		iio_device_release_direct_mode(indio_dev);
+		return ret;
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		return sx9310_read_samp_freq(data, val, val2);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int sx9310_set_samp_freq(struct sx9310_data *data, int val, int val2)
+{
+	int i, ret;
+
+	for (i = 0; i < ARRAY_SIZE(sx9310_samp_freq_table); i++)
+		if (val == sx9310_samp_freq_table[i].val &&
+		    val2 == sx9310_samp_freq_table[i].val2)
+			break;
+
+	if (i == ARRAY_SIZE(sx9310_samp_freq_table))
+		return -EINVAL;
+
+	mutex_lock(&data->mutex);
+
+	ret = regmap_update_bits(data->regmap, SX9310_REG_PROX_CTRL0,
+				 SX9310_SCAN_PERIOD_MASK,
+				 i << SX9310_SCAN_PERIOD_SHIFT);
+
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static int sx9310_write_raw(struct iio_dev *indio_dev,
+			    const struct iio_chan_spec *chan, int val, int val2,
+			    long mask)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+
+	if (chan->type != IIO_PROXIMITY)
+		return -EINVAL;
+
+	if (mask != IIO_CHAN_INFO_SAMP_FREQ)
+		return -EINVAL;
+
+	return sx9310_set_samp_freq(data, val, val2);
+}
+
+static irqreturn_t sx9310_irq_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct sx9310_data *data = iio_priv(indio_dev);
+
+	if (data->trigger_enabled)
+		iio_trigger_poll(data->trig);
+
+	/*
+	 * Even if no event is enabled, we need to wake the thread to
+	 * clear the interrupt state by reading SX9310_REG_IRQ_SRC.  It
+	 * is not possible to do that here because regmap_read takes a
+	 * mutex.
+	 */
+	return IRQ_WAKE_THREAD;
+}
+
+static void sx9310_push_events(struct iio_dev *indio_dev)
+{
+	int ret;
+	unsigned int val, chan;
+	struct sx9310_data *data = iio_priv(indio_dev);
+	s64 timestamp = iio_get_time_ns(indio_dev);
+
+	/* Read proximity state on all channels */
+	ret = regmap_read(data->regmap, SX9310_REG_STAT0, &val);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "i2c transfer error in irq\n");
+		return;
+	}
+
+	for (chan = 0; chan < SX9310_NUM_CHANNELS; chan++) {
+		int dir;
+		u64 ev;
+		bool new_prox = val & BIT(chan);
+
+		if (!(data->chan_event & BIT(chan)))
+			continue;
+		if (new_prox == data->prox_stat[chan])
+			/* No change on this channel. */
+			continue;
+
+		dir = new_prox ? IIO_EV_DIR_FALLING : IIO_EV_DIR_RISING;
+		ev = IIO_UNMOD_EVENT_CODE(IIO_PROXIMITY, chan,
+					  IIO_EV_TYPE_THRESH, dir);
+
+		iio_push_event(indio_dev, ev, timestamp);
+		data->prox_stat[chan] = new_prox;
+	}
+}
+
+static irqreturn_t sx9310_irq_thread_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int ret;
+	unsigned int val;
+
+	mutex_lock(&data->mutex);
+
+	ret = regmap_read(data->regmap, SX9310_REG_IRQ_SRC, &val);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "i2c transfer error in irq\n");
+		goto out;
+	}
+
+	if (val & SX9310_EVENT_IRQ)
+		sx9310_push_events(indio_dev);
+
+	if (val & SX9310_CONVDONE_IRQ)
+		complete(&data->completion);
+
+out:
+	mutex_unlock(&data->mutex);
+
+	return IRQ_HANDLED;
+}
+
+static int sx9310_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 sx9310_data *data = iio_priv(indio_dev);
+
+	return !!(data->chan_event & BIT(chan->channel));
+}
+
+static int sx9310_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 sx9310_data *data = iio_priv(indio_dev);
+	int ret;
+
+	/* If the state hasn't changed, there's nothing to do. */
+	if (!!(data->chan_event & BIT(chan->channel)) == state)
+		return 0;
+
+	mutex_lock(&data->mutex);
+	if (state) {
+		ret = sx9310_get_event_channel(data, chan->channel);
+		if (ret < 0)
+			goto out_unlock;
+		if (!(data->chan_event & ~BIT(chan->channel))) {
+			ret = sx9310_enable_irq(data, SX9310_EVENT_IRQ);
+			if (ret < 0)
+				sx9310_put_event_channel(data, chan->channel);
+		}
+	} else {
+		ret = sx9310_put_event_channel(data, chan->channel);
+		if (ret < 0)
+			goto out_unlock;
+		if (!data->chan_event) {
+			ret = sx9310_disable_irq(data, SX9310_EVENT_IRQ);
+			if (ret < 0)
+				sx9310_get_event_channel(data, chan->channel);
+		}
+	}
+
+out_unlock:
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static struct attribute *sx9310_attributes[] = {
+	&iio_dev_attr_sampling_frequency_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group sx9310_attribute_group = {
+	.attrs = sx9310_attributes,
+};
+
+static const struct iio_info sx9310_info = {
+	.attrs = &sx9310_attribute_group,
+	.read_raw = sx9310_read_raw,
+	.write_raw = sx9310_write_raw,
+	.read_event_config = sx9310_read_event_config,
+	.write_event_config = sx9310_write_event_config,
+};
+
+static int sx9310_set_trigger_state(struct iio_trigger *trig, bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int ret = 0;
+
+	mutex_lock(&data->mutex);
+
+	if (state)
+		ret = sx9310_enable_irq(data, SX9310_CONVDONE_IRQ);
+	else if (!data->chan_read)
+		ret = sx9310_disable_irq(data, SX9310_CONVDONE_IRQ);
+	if (ret < 0)
+		goto out;
+
+	data->trigger_enabled = state;
+
+out:
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static const struct iio_trigger_ops sx9310_trigger_ops = {
+	.set_trigger_state = sx9310_set_trigger_state,
+};
+
+static irqreturn_t sx9310_trigger_handler(int irq, void *private)
+{
+	struct iio_poll_func *pf = private;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct sx9310_data *data = iio_priv(indio_dev);
+	__be16 val;
+	int bit, ret, i = 0;
+
+	mutex_lock(&data->mutex);
+
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->masklength) {
+		ret = sx9310_read_prox_data(data, &indio_dev->channels[bit],
+					    &val);
+		if (ret < 0)
+			goto out;
+
+		data->buffer[i++] = val;
+	}
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+					   pf->timestamp);
+
+out:
+	mutex_unlock(&data->mutex);
+
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int sx9310_buffer_preenable(struct iio_dev *indio_dev)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+	unsigned int channels = 0;
+	int bit, ret;
+
+	mutex_lock(&data->mutex);
+	for_each_set_bit(bit, indio_dev->active_scan_mask,
+			 indio_dev->masklength)
+		channels |= BIT(indio_dev->channels[bit].channel);
+
+	ret = sx9310_update_chan_en(data, channels, data->chan_event);
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static int sx9310_buffer_postdisable(struct iio_dev *indio_dev)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = sx9310_update_chan_en(data, 0, data->chan_event);
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static const struct iio_buffer_setup_ops sx9310_buffer_setup_ops = {
+	.preenable = sx9310_buffer_preenable,
+	.postenable = iio_triggered_buffer_postenable,
+	.predisable = iio_triggered_buffer_predisable,
+	.postdisable = sx9310_buffer_postdisable,
+};
+
+struct sx9310_reg_default {
+	u8 reg;
+	u8 def;
+};
+
+#define SX_INIT(_reg, _def)			\
+	{					\
+		.reg = SX9310_REG_##_reg,	\
+		.def = _def,			\
+	}
+
+static const struct sx9310_reg_default sx9310_default_regs[] = {
+	SX_INIT(IRQ_MSK, 0x00),
+	SX_INIT(IRQ_FUNC, 0x00),
+	/*
+	 * The lower 4 bits should not be set as it enable sensors measurements.
+	 * Turning the detection on before the configuration values are set to
+	 * good values can cause the device to return erroneous readings.
+	 */
+	SX_INIT(PROX_CTRL0, SX9310_REG_PROX_CTRL0_PROXSTAT2),
+	SX_INIT(PROX_CTRL1, 0x00),
+	SX_INIT(PROX_CTRL2, SX9310_REG_PROX_CTRL2_COMBMODE_ALL |
+			    SX9310_REG_PROX_CTRL2_SHIELDEN_DYNAMIC),
+	SX_INIT(PROX_CTRL3, SX9310_REG_PROX_CTRL3_GAIN0_X8 |
+			    SX9310_REG_PROX_CTRL3_GAIN12_X4),
+	SX_INIT(PROX_CTRL4, SX9310_REG_PROX_CTRL4_RESOLUTION_FINEST),
+	SX_INIT(PROX_CTRL5, SX9310_REG_PROX_CTRL5_RANGE_SMALL |
+			    SX9310_REG_PROX_CTRL5_STARTUPSENS_CS1 |
+			    SX9310_REG_PROX_CTRL5_RAWFILT_1P25),
+	SX_INIT(PROX_CTRL6, SX9310_REG_PROX_CTRL6_COMP_COMMON),
+	SX_INIT(PROX_CTRL7, SX9310_REG_PROX_CTRL7_AVGNEGFILT_2 |
+			    SX9310_REG_PROX_CTRL7_AVGPOSFILT_512),
+	SX_INIT(PROX_CTRL8, SX9310_REG_PROX_CTRL8_9_PTHRESH_96 |
+			    SX9310_REG_PROX_CTRL8_9_BODYTHRESH_1500),
+	SX_INIT(PROX_CTRL9, SX9310_REG_PROX_CTRL8_9_PTHRESH12_28 |
+			    SX9310_REG_PROX_CTRL8_9_BODYTHRESH_900),
+	SX_INIT(PROX_CTRL10, SX9310_REG_PROX_CTRL10_HYST_6PCT |
+			     SX9310_REG_PROX_CTRL10_CLOSE_DEBOUNCE_8 |
+			     SX9310_REG_PROX_CTRL10_FAR_DEBOUNCE_8),
+	SX_INIT(PROX_CTRL11, 0x00),
+	SX_INIT(PROX_CTRL12, 0x00),
+	SX_INIT(PROX_CTRL13, 0x00),
+	SX_INIT(PROX_CTRL14, 0x00),
+	SX_INIT(PROX_CTRL15, 0x00),
+	SX_INIT(PROX_CTRL16, 0x00),
+	SX_INIT(PROX_CTRL17, 0x00),
+	SX_INIT(PROX_CTRL18, 0x00),
+	SX_INIT(PROX_CTRL19, 0x00),
+	SX_INIT(SAR_CTRL0, SX9310_REG_SAR_CTRL0_SARDEB_4_SAMPLES |
+			   SX9310_REG_SAR_CTRL0_SARHYST_8),
+	SX_INIT(SAR_CTRL1, SX9310_REG_SAR_CTRL1_SLOPE(10781250)),
+	SX_INIT(SAR_CTRL2, SX9310_REG_SAR_CTRL2_SAROFFSET_DEFAULT),
+};
+
+/* Activate all channels and perform an initial compensation. */
+static int sx9310_init_compensation(struct iio_dev *indio_dev)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int i, ret;
+	unsigned int val;
+	unsigned int ctrl0;
+
+	ret = regmap_read(data->regmap, SX9310_REG_PROX_CTRL0, &ctrl0);
+	if (ret < 0)
+		return ret;
+
+	/* run the compensation phase on all channels */
+	ret = regmap_write(data->regmap, SX9310_REG_PROX_CTRL0,
+			   ctrl0 | SX9310_REG_PROX_CTRL0_EN_MASK);
+	if (ret < 0)
+		return ret;
+
+	for (i = 100; i >= 0; i--) {
+		msleep(20);
+		ret = regmap_read(data->regmap, SX9310_REG_STAT1, &val);
+		if (ret < 0)
+			goto out;
+		if (!(val & SX9310_COMPSTAT_MASK))
+			break;
+	}
+
+	if (i < 0) {
+		dev_err(&data->client->dev,
+			"initial compensation timed out: 0x%02x", val);
+		ret = -ETIMEDOUT;
+	}
+
+out:
+	regmap_write(data->regmap, SX9310_REG_PROX_CTRL0, ctrl0);
+	return ret;
+}
+
+static int sx9310_init_device(struct iio_dev *indio_dev)
+{
+	struct sx9310_data *data = iio_priv(indio_dev);
+	const struct sx9310_reg_default *initval;
+	int ret;
+	unsigned int i, val;
+
+	ret = regmap_write(data->regmap, SX9310_REG_RESET, SX9310_SOFT_RESET);
+	if (ret < 0)
+		return ret;
+
+	usleep_range(1000, 2000); /* power-up time is ~1ms. */
+
+	/* Clear reset interrupt state by reading SX9310_REG_IRQ_SRC. */
+	ret = regmap_read(data->regmap, SX9310_REG_IRQ_SRC, &val);
+	if (ret < 0)
+		return ret;
+
+	/* Program some sane defaults. */
+	for (i = 0; i < ARRAY_SIZE(sx9310_default_regs); i++) {
+		initval = &sx9310_default_regs[i];
+		ret = regmap_write(data->regmap, initval->reg, initval->def);
+		if (ret < 0)
+			return ret;
+	}
+
+	return sx9310_init_compensation(indio_dev);
+}
+
+static int sx9310_set_indio_dev_name(struct device *dev,
+				     struct iio_dev *indio_dev,
+				     const struct i2c_device_id *id, int whoami)
+{
+	const struct acpi_device_id *acpi_id;
+
+	/* id will be NULL when enumerated via ACPI */
+	if (id) {
+		if (id->driver_data != whoami)
+			dev_err(dev, "WHOAMI does not match i2c_device_id: %s",
+				id->name);
+	} else if (ACPI_HANDLE(dev)) {
+		acpi_id = acpi_match_device(dev->driver->acpi_match_table, dev);
+		if (!acpi_id)
+			return -ENODEV;
+		if (acpi_id->driver_data != whoami)
+			dev_err(dev, "WHOAMI does not match acpi_device_id: %s",
+				acpi_id->id);
+	} else
+		return -ENODEV;
+
+	switch (whoami) {
+	case SX9310_WHOAMI_VALUE:
+		indio_dev->name = "sx9310";
+		break;
+	case SX9311_WHOAMI_VALUE:
+		indio_dev->name = "sx9311";
+		break;
+	default:
+		dev_err(dev, "unexpected WHOAMI response: %u", whoami);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int sx9310_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	int ret;
+	struct iio_dev *indio_dev;
+	struct sx9310_data *data;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (indio_dev == NULL)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	data->client = client;
+	mutex_init(&data->mutex);
+	init_completion(&data->completion);
+
+	data->regmap = devm_regmap_init_i2c(client, &sx9310_regmap_config);
+	if (IS_ERR(data->regmap))
+		return PTR_ERR(data->regmap);
+
+	ret = regmap_read(data->regmap, SX9310_REG_WHOAMI, &data->whoami);
+	if (ret < 0) {
+		dev_err(&client->dev, "error in reading WHOAMI register: %d",
+			ret);
+		return ret;
+	}
+
+	ret = sx9310_set_indio_dev_name(&client->dev, indio_dev, id,
+					data->whoami);
+	if (ret < 0)
+		return ret;
+
+	ACPI_COMPANION_SET(&indio_dev->dev, ACPI_COMPANION(&client->dev));
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->channels = sx9310_channels;
+	indio_dev->num_channels = ARRAY_SIZE(sx9310_channels);
+	indio_dev->info = &sx9310_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	i2c_set_clientdata(client, indio_dev);
+
+	ret = sx9310_init_device(indio_dev);
+	if (ret < 0)
+		return ret;
+
+	if (client->irq) {
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						sx9310_irq_handler,
+						sx9310_irq_thread_handler,
+						IRQF_TRIGGER_LOW | IRQF_ONESHOT,
+						"sx9310_event", indio_dev);
+		if (ret < 0)
+			return ret;
+
+		data->trig =
+			devm_iio_trigger_alloc(&client->dev, "%s-dev%d",
+					       indio_dev->name, indio_dev->id);
+		if (!data->trig)
+			return -ENOMEM;
+
+		data->trig->dev.parent = &client->dev;
+		data->trig->ops = &sx9310_trigger_ops;
+		iio_trigger_set_drvdata(data->trig, indio_dev);
+
+		ret = devm_iio_trigger_register(&client->dev, data->trig);
+		if (ret)
+			return ret;
+	}
+
+	ret = devm_iio_triggered_buffer_setup(&client->dev, indio_dev,
+					      iio_pollfunc_store_time,
+					      sx9310_trigger_handler,
+					      &sx9310_buffer_setup_ops);
+	if (ret < 0)
+		return ret;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static int __maybe_unused sx9310_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct sx9310_data *data = iio_priv(indio_dev);
+	u8 ctrl0;
+	int ret;
+
+	disable_irq_nosync(data->client->irq);
+
+	mutex_lock(&data->mutex);
+	ret = regmap_read(data->regmap, SX9310_REG_PROX_CTRL0,
+			  &data->suspend_ctrl0);
+
+	if (ret)
+		goto out;
+
+	ctrl0 = data->suspend_ctrl0 & ~SX9310_REG_PROX_CTRL0_EN_MASK;
+	ret = regmap_write(data->regmap, SX9310_REG_PROX_CTRL0, ctrl0);
+	if (ret)
+		goto out;
+
+	ret = regmap_write(data->regmap, SX9310_REG_PAUSE, 0);
+
+out:
+	mutex_unlock(&data->mutex);
+	return ret;
+}
+
+static int __maybe_unused sx9310_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct sx9310_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = regmap_write(data->regmap, SX9310_REG_PAUSE, 1);
+	if (ret)
+		goto out;
+
+	ret = regmap_write(data->regmap, SX9310_REG_PROX_CTRL0,
+			   data->suspend_ctrl0);
+
+out:
+	mutex_unlock(&data->mutex);
+
+	enable_irq(data->client->irq);
+
+	return ret;
+}
+
+static const struct dev_pm_ops sx9310_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(sx9310_suspend, sx9310_resume)
+};
+
+static const struct acpi_device_id sx9310_acpi_match[] = {
+	{ "STH9310", SX9310_WHOAMI_VALUE },
+	{ "STH9311", SX9311_WHOAMI_VALUE },
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, sx9310_acpi_match);
+
+static const struct of_device_id sx9310_of_match[] = {
+	{ .compatible = "semtech,sx9310" },
+	{ .compatible = "semtech,sx9311" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, sx9310_of_match);
+
+static const struct i2c_device_id sx9310_id[] = {
+	{ "sx9310", SX9310_WHOAMI_VALUE },
+	{ "sx9311", SX9311_WHOAMI_VALUE },
+	{},
+};
+MODULE_DEVICE_TABLE(i2c, sx9310_id);
+
+static struct i2c_driver sx9310_driver = {
+	.driver = {
+		.name	= "sx9310",
+		.acpi_match_table = ACPI_PTR(sx9310_acpi_match),
+		.of_match_table = of_match_ptr(sx9310_of_match),
+		.pm = &sx9310_pm_ops,
+	},
+	.probe		= sx9310_probe,
+	.id_table	= sx9310_id,
+};
+module_i2c_driver(sx9310_driver);
+
+MODULE_AUTHOR("Gwendal Grignou <gwendal@chromium.org>");
+MODULE_AUTHOR("Daniel Campello <campello@chromium.org>");
+MODULE_DESCRIPTION("Driver for Semtech SX9310/SX9311 proximity sensor");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/proximity/vcnl3020.c b/drivers/iio/proximity/vcnl3020.c
new file mode 100644
index 000000000000..9ff1a164c2e6
--- /dev/null
+++ b/drivers/iio/proximity/vcnl3020.c
@@ -0,0 +1,258 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Support for Vishay VCNL3020 proximity sensor on i2c bus.
+ * Based on Vishay VCNL4000 driver code.
+ *
+ * TODO: interrupts.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+#include <linux/delay.h>
+#include <linux/regmap.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define VCNL3020_PROD_ID	0x21
+
+#define VCNL_COMMAND		0x80 /* Command register */
+#define VCNL_PROD_REV		0x81 /* Product ID and Revision ID */
+#define VCNL_PROXIMITY_RATE	0x82 /* Rate of Proximity Measurement */
+#define VCNL_LED_CURRENT	0x83 /* IR LED current for proximity mode */
+#define VCNL_PS_RESULT_HI	0x87 /* Proximity result register, MSB */
+#define VCNL_PS_RESULT_LO	0x88 /* Proximity result register, LSB */
+#define VCNL_PS_ICR		0x89 /* Interrupt Control Register */
+#define VCNL_PS_LO_THR_HI	0x8a /* High byte of low threshold value */
+#define VCNL_PS_LO_THR_LO	0x8b /* Low byte of low threshold value */
+#define VCNL_PS_HI_THR_HI	0x8c /* High byte of high threshold value */
+#define VCNL_PS_HI_THR_LO	0x8d /* Low byte of high threshold value */
+#define VCNL_ISR		0x8e /* Interrupt Status Register */
+#define VCNL_PS_MOD_ADJ		0x8f /* Proximity Modulator Timing Adjustment */
+
+/* Bit masks for COMMAND register */
+#define VCNL_PS_RDY		BIT(5) /* proximity data ready? */
+#define VCNL_PS_OD		BIT(3) /* start on-demand proximity
+					* measurement
+					*/
+
+#define VCNL_ON_DEMAND_TIMEOUT_US	100000
+#define VCNL_POLL_US			20000
+
+/**
+ * struct vcnl3020_data - vcnl3020 specific data.
+ * @regmap:	device register map.
+ * @dev:	vcnl3020 device.
+ * @rev:	revision id.
+ * @lock:	lock for protecting access to device hardware registers.
+ */
+struct vcnl3020_data {
+	struct regmap *regmap;
+	struct device *dev;
+	u8 rev;
+	struct mutex lock;
+};
+
+/**
+ * struct vcnl3020_property - vcnl3020 property.
+ * @name:	property name.
+ * @reg:	i2c register offset.
+ * @conversion_func:	conversion function.
+ */
+struct vcnl3020_property {
+	const char *name;
+	u32 reg;
+	u32 (*conversion_func)(u32 *val);
+};
+
+static u32 microamp_to_reg(u32 *val)
+{
+	/*
+	 * An example of conversion from uA to reg val:
+	 * 200000 uA == 200 mA == 20
+	 */
+	return *val /= 10000;
+};
+
+static struct vcnl3020_property vcnl3020_led_current_property = {
+	.name = "vishay,led-current-microamp",
+	.reg = VCNL_LED_CURRENT,
+	.conversion_func = microamp_to_reg,
+};
+
+static int vcnl3020_get_and_apply_property(struct vcnl3020_data *data,
+					   struct vcnl3020_property prop)
+{
+	int rc;
+	u32 val;
+
+	rc = device_property_read_u32(data->dev, prop.name, &val);
+	if (rc)
+		return 0;
+
+	if (prop.conversion_func)
+		prop.conversion_func(&val);
+
+	rc = regmap_write(data->regmap, prop.reg, val);
+	if (rc) {
+		dev_err(data->dev, "Error (%d) setting property (%s)\n",
+			rc, prop.name);
+	}
+
+	return rc;
+}
+
+static int vcnl3020_init(struct vcnl3020_data *data)
+{
+	int rc;
+	unsigned int reg;
+
+	rc = regmap_read(data->regmap, VCNL_PROD_REV, &reg);
+	if (rc) {
+		dev_err(data->dev,
+			"Error (%d) reading product revision\n", rc);
+		return rc;
+	}
+
+	if (reg != VCNL3020_PROD_ID) {
+		dev_err(data->dev,
+			"Product id (%x) did not match vcnl3020 (%x)\n", reg,
+			VCNL3020_PROD_ID);
+		return -ENODEV;
+	}
+
+	data->rev = reg;
+	mutex_init(&data->lock);
+
+	return vcnl3020_get_and_apply_property(data,
+					       vcnl3020_led_current_property);
+};
+
+static int vcnl3020_measure_proximity(struct vcnl3020_data *data, int *val)
+{
+	int rc;
+	unsigned int reg;
+	__be16 res;
+
+	mutex_lock(&data->lock);
+
+	rc = regmap_write(data->regmap, VCNL_COMMAND, VCNL_PS_OD);
+	if (rc)
+		goto err_unlock;
+
+	/* wait for data to become ready */
+	rc = regmap_read_poll_timeout(data->regmap, VCNL_COMMAND, reg,
+				      reg & VCNL_PS_RDY, VCNL_POLL_US,
+				      VCNL_ON_DEMAND_TIMEOUT_US);
+	if (rc) {
+		dev_err(data->dev,
+			"Error (%d) reading vcnl3020 command register\n", rc);
+		goto err_unlock;
+	}
+
+	/* high & low result bytes read */
+	rc = regmap_bulk_read(data->regmap, VCNL_PS_RESULT_HI, &res,
+			      sizeof(res));
+	if (rc)
+		goto err_unlock;
+
+	*val = be16_to_cpu(res);
+
+err_unlock:
+	mutex_unlock(&data->lock);
+
+	return rc;
+}
+
+static const struct iio_chan_spec vcnl3020_channels[] = {
+	{
+		.type = IIO_PROXIMITY,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
+	},
+};
+
+static int vcnl3020_read_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan, int *val,
+			     int *val2, long mask)
+{
+	int rc;
+	struct vcnl3020_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		rc = vcnl3020_measure_proximity(data, val);
+		if (rc)
+			return rc;
+		return IIO_VAL_INT;
+	default:
+		return -EINVAL;
+	}
+}
+
+static const struct iio_info vcnl3020_info = {
+	.read_raw = vcnl3020_read_raw,
+};
+
+static const struct regmap_config vcnl3020_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.max_register	= VCNL_PS_MOD_ADJ,
+};
+
+static int vcnl3020_probe(struct i2c_client *client)
+{
+	struct vcnl3020_data *data;
+	struct iio_dev *indio_dev;
+	struct regmap *regmap;
+	int rc;
+
+	regmap = devm_regmap_init_i2c(client, &vcnl3020_regmap_config);
+	if (IS_ERR(regmap)) {
+		dev_err(&client->dev, "regmap_init failed\n");
+		return PTR_ERR(regmap);
+	}
+
+	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->regmap = regmap;
+	data->dev = &client->dev;
+
+	rc = vcnl3020_init(data);
+	if (rc)
+		return rc;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &vcnl3020_info;
+	indio_dev->channels = vcnl3020_channels;
+	indio_dev->num_channels = ARRAY_SIZE(vcnl3020_channels);
+	indio_dev->name = "vcnl3020";
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static const struct of_device_id vcnl3020_of_match[] = {
+	{
+		.compatible = "vishay,vcnl3020",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, vcnl3020_of_match);
+
+static struct i2c_driver vcnl3020_driver = {
+	.driver = {
+		.name   = "vcnl3020",
+		.of_match_table = vcnl3020_of_match,
+	},
+	.probe_new  = vcnl3020_probe,
+};
+module_i2c_driver(vcnl3020_driver);
+
+MODULE_AUTHOR("Ivan Mikhaylov <i.mikhaylov@yadro.com>");
+MODULE_DESCRIPTION("Vishay VCNL3020 proximity sensor driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/iio/temperature/hid-sensor-temperature.c b/drivers/iio/temperature/hid-sensor-temperature.c
index eda55b9c1e9b..8d1f434f109d 100644
--- a/drivers/iio/temperature/hid-sensor-temperature.c
+++ b/drivers/iio/temperature/hid-sensor-temperature.c
@@ -7,8 +7,6 @@
 #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>
 
@@ -230,12 +228,8 @@ static int hid_temperature_probe(struct platform_device *pdev)
 	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)
@@ -258,7 +252,7 @@ static int hid_temperature_probe(struct platform_device *pdev)
 error_remove_callback:
 	sensor_hub_remove_callback(hsdev, HID_USAGE_SENSOR_TEMPERATURE);
 error_remove_trigger:
-	hid_sensor_remove_trigger(&temp_st->common_attributes);
+	hid_sensor_remove_trigger(indio_dev, &temp_st->common_attributes);
 	return ret;
 }
 
@@ -270,7 +264,7 @@ static int hid_temperature_remove(struct platform_device *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);
+	hid_sensor_remove_trigger(indio_dev, &temp_st->common_attributes);
 
 	return 0;
 }
diff --git a/drivers/iio/temperature/ltc2983.c b/drivers/iio/temperature/ltc2983.c
index d39c0d6b77f1..8976e8d59826 100644
--- a/drivers/iio/temperature/ltc2983.c
+++ b/drivers/iio/temperature/ltc2983.c
@@ -390,8 +390,8 @@ static struct ltc2983_custom_sensor *__ltc2983_custom_sensor_new(
 	 * For custom steinhart, the full u32 is taken. For all the others
 	 * the MSB is discarded.
 	 */
-	const u8 n_size = (is_steinhart == true) ? 4 : 3;
-	const u8 e_size = (is_steinhart == true) ? sizeof(u32) : sizeof(u64);
+	const u8 n_size = is_steinhart ? 4 : 3;
+	const u8 e_size = is_steinhart ? sizeof(u32) : sizeof(u64);
 
 	n_entries = of_property_count_elems_of_size(np, propname, e_size);
 	/* n_entries must be an even number */
diff --git a/drivers/iio/temperature/max31856.c b/drivers/iio/temperature/max31856.c
index b4cb21ab2e85..b4c49a5d3685 100644
--- a/drivers/iio/temperature/max31856.c
+++ b/drivers/iio/temperature/max31856.c
@@ -14,6 +14,7 @@
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/util_macros.h>
+#include <asm/unaligned.h>
 #include <dt-bindings/iio/temperature/thermocouple.h>
 /*
  * The MSB of the register value determines whether the following byte will
@@ -168,7 +169,7 @@ static int max31856_thermocouple_read(struct max31856_data *data,
 		if (ret)
 			return ret;
 		/* Skip last 5 dead bits of LTCBL */
-		*val = (reg_val[0] << 16 | reg_val[1] << 8 | reg_val[2]) >> 5;
+		*val = get_unaligned_be24(&reg_val[0]) >> 5;
 		/* Check 7th bit of LTCBH reg. value for sign*/
 		if (reg_val[0] & 0x80)
 			*val -= 0x80000;
@@ -185,7 +186,7 @@ static int max31856_thermocouple_read(struct max31856_data *data,
 		/* Get Cold Junction Temp. offset register value */
 		offset_cjto = reg_val[0];
 		/* Get CJTH and CJTL value and skip last 2 dead bits of CJTL */
-		*val = (reg_val[1] << 8 | reg_val[2]) >> 2;
+		*val = get_unaligned_be16(&reg_val[1]) >> 2;
 		/* As per datasheet add offset into CJTH and CJTL */
 		*val += offset_cjto;
 		/* Check 7th bit of CJTH reg. value for sign */
diff --git a/drivers/iio/trigger/iio-trig-hrtimer.c b/drivers/iio/trigger/iio-trig-hrtimer.c
index a5e670726717..f59bf8d58586 100644
--- a/drivers/iio/trigger/iio-trig-hrtimer.c
+++ b/drivers/iio/trigger/iio-trig-hrtimer.c
@@ -4,7 +4,7 @@
  *
  * Copyright (C) Intuitive Aerial AB
  * Written by Marten Svanfeldt, marten@intuitiveaerial.com
- * Copyright (C) 2012, Analog Device Inc.
+ * Copyright (C) 2012, Analog Devices Inc.
  *	Author: Lars-Peter Clausen <lars@metafoo.de>
  * Copyright (C) 2015, Intel Corporation
  */
diff --git a/drivers/staging/android/ion/ion_page_pool.c b/drivers/staging/android/ion/ion_page_pool.c
index f85ec5b16b65..0198b886d906 100644
--- a/drivers/staging/android/ion/ion_page_pool.c
+++ b/drivers/staging/android/ion/ion_page_pool.c
@@ -37,7 +37,7 @@ static void ion_page_pool_add(struct ion_page_pool *pool, struct page *page)
 	}
 
 	mod_node_page_state(page_pgdat(page), NR_KERNEL_MISC_RECLAIMABLE,
-							1 << pool->order);
+			    1 << pool->order);
 	mutex_unlock(&pool->mutex);
 }
 
@@ -57,7 +57,7 @@ static struct page *ion_page_pool_remove(struct ion_page_pool *pool, bool high)
 
 	list_del(&page->lru);
 	mod_node_page_state(page_pgdat(page), NR_KERNEL_MISC_RECLAIMABLE,
-							-(1 << pool->order));
+			    -(1 << pool->order));
 	return page;
 }
 
diff --git a/drivers/staging/axis-fifo/axis-fifo.c b/drivers/staging/axis-fifo/axis-fifo.c
index 5801067e7c1b..2bb1c2e9cb57 100644
--- a/drivers/staging/axis-fifo/axis-fifo.c
+++ b/drivers/staging/axis-fifo/axis-fifo.c
@@ -383,8 +383,9 @@ static ssize_t axis_fifo_read(struct file *f, char __user *buf,
 		mutex_lock(&fifo->read_lock);
 		ret = wait_event_interruptible_timeout(fifo->read_queue,
 			ioread32(fifo->base_addr + XLLF_RDFO_OFFSET),
-			(read_timeout >= 0) ? msecs_to_jiffies(read_timeout) :
-				MAX_SCHEDULE_TIMEOUT);
+				 (read_timeout >= 0) ?
+				  msecs_to_jiffies(read_timeout) :
+				  MAX_SCHEDULE_TIMEOUT);
 
 		if (ret <= 0) {
 			if (ret == 0) {
@@ -525,9 +526,10 @@ static ssize_t axis_fifo_write(struct file *f, const char __user *buf,
 		mutex_lock(&fifo->write_lock);
 		ret = wait_event_interruptible_timeout(fifo->write_queue,
 			ioread32(fifo->base_addr + XLLF_TDFV_OFFSET)
-				>= words_to_write,
-			(write_timeout >= 0) ? msecs_to_jiffies(write_timeout) :
-				MAX_SCHEDULE_TIMEOUT);
+				 >= words_to_write,
+				 (write_timeout >= 0) ?
+				  msecs_to_jiffies(write_timeout) :
+				  MAX_SCHEDULE_TIMEOUT);
 
 		if (ret <= 0) {
 			if (ret == 0) {
diff --git a/drivers/staging/comedi/comedi_internal.h b/drivers/staging/comedi/comedi_internal.h
index 7c8f18f55122..9b3631a654c8 100644
--- a/drivers/staging/comedi/comedi_internal.h
+++ b/drivers/staging/comedi/comedi_internal.h
@@ -32,8 +32,8 @@ 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);
+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);
 unsigned int comedi_buf_write_n_allocated(struct comedi_subdevice *s);
 void comedi_device_cancel_all(struct comedi_device *dev);
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index 7c82d5f9778f..c1d70eec24ab 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1214,7 +1214,7 @@ static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 	struct ni_private *devpriv = dev->private;
 	struct mite *mite = devpriv->mite;
 	resource_size_t daq_phys_addr;
-	static const int Start_Cal_EEPROM = 0x400;
+	static const int start_cal_eeprom = 0x400;
 	static const unsigned int window_size = 10;
 	unsigned int old_iodwbsr_bits;
 	unsigned int old_iodwbsr1_bits;
@@ -1234,7 +1234,7 @@ static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 	writel(0xf, mite->mmio + 0x30);
 
 	for (i = 0; i < M_SERIES_EEPROM_SIZE; ++i)
-		devpriv->eeprom_buffer[i] = ni_readb(dev, Start_Cal_EEPROM + i);
+		devpriv->eeprom_buffer[i] = ni_readb(dev, start_cal_eeprom + i);
 
 	writel(old_iodwbsr1_bits, mite->mmio + MITE_IODWBSR_1);
 	writel(old_iodwbsr_bits, mite->mmio + MITE_IODWBSR);
diff --git a/drivers/staging/fbtft/fb_st7789v.c b/drivers/staging/fbtft/fb_st7789v.c
index 3c3f387936e8..3a280cc1892c 100644
--- a/drivers/staging/fbtft/fb_st7789v.c
+++ b/drivers/staging/fbtft/fb_st7789v.c
@@ -20,6 +20,12 @@
 	"70 2C 2E 15 10 09 48 33 53 0B 19 18 20 25\n" \
 	"70 2C 2E 15 10 09 48 33 53 0B 19 18 20 25"
 
+#define HSD20_IPS_GAMMA \
+	"D0 05 0A 09 08 05 2E 44 45 0F 17 16 2B 33\n" \
+	"D0 05 0A 09 08 05 2E 43 45 0F 16 16 2B 33"
+
+#define HSD20_IPS 1
+
 /**
  * enum st7789v_command - ST7789V display controller commands
  *
@@ -82,14 +88,20 @@ static int init_display(struct fbtft_par *par)
 
 	/* set pixel format to RGB-565 */
 	write_reg(par, MIPI_DCS_SET_PIXEL_FORMAT, MIPI_DCS_PIXEL_FMT_16BIT);
+	if (HSD20_IPS)
+		write_reg(par, PORCTRL, 0x05, 0x05, 0x00, 0x33, 0x33);
 
-	write_reg(par, PORCTRL, 0x08, 0x08, 0x00, 0x22, 0x22);
+	else
+		write_reg(par, PORCTRL, 0x08, 0x08, 0x00, 0x22, 0x22);
 
 	/*
 	 * VGH = 13.26V
 	 * VGL = -10.43V
 	 */
-	write_reg(par, GCTRL, 0x35);
+	if (HSD20_IPS)
+		write_reg(par, GCTRL, 0x75);
+	else
+		write_reg(par, GCTRL, 0x35);
 
 	/*
 	 * VDV and VRH register values come from command write
@@ -101,13 +113,19 @@ static int init_display(struct fbtft_par *par)
 	 * VAP =  4.1V + (VCOM + VCOM offset + 0.5 * VDV)
 	 * VAN = -4.1V + (VCOM + VCOM offset + 0.5 * VDV)
 	 */
-	write_reg(par, VRHS, 0x0B);
+	if (HSD20_IPS)
+		write_reg(par, VRHS, 0x13);
+	else
+		write_reg(par, VRHS, 0x0B);
 
 	/* VDV = 0V */
 	write_reg(par, VDVS, 0x20);
 
 	/* VCOM = 0.9V */
-	write_reg(par, VCOMS, 0x20);
+	if (HSD20_IPS)
+		write_reg(par, VCOMS, 0x22);
+	else
+		write_reg(par, VCOMS, 0x20);
 
 	/* VCOM offset = 0V */
 	write_reg(par, VCMOFSET, 0x20);
@@ -120,6 +138,10 @@ static int init_display(struct fbtft_par *par)
 	write_reg(par, PWCTRL1, 0xA4, 0xA1);
 
 	write_reg(par, MIPI_DCS_SET_DISPLAY_ON);
+
+	if (HSD20_IPS)
+		write_reg(par, MIPI_DCS_ENTER_INVERT_MODE);
+
 	return 0;
 }
 
@@ -234,7 +256,7 @@ static struct fbtft_display display = {
 	.height = 320,
 	.gamma_num = 2,
 	.gamma_len = 14,
-	.gamma = DEFAULT_GAMMA,
+	.gamma = HSD20_IPS_GAMMA,
 	.fbtftops = {
 		.init_display = init_display,
 		.set_var = set_var,
diff --git a/drivers/staging/fsl-dpaa2/ethsw/ethsw.c b/drivers/staging/fsl-dpaa2/ethsw/ethsw.c
index 676d1ad1b50d..546ad376df99 100644
--- a/drivers/staging/fsl-dpaa2/ethsw/ethsw.c
+++ b/drivers/staging/fsl-dpaa2/ethsw/ethsw.c
@@ -1094,7 +1094,8 @@ static int swdev_port_obj_del(struct net_device *netdev,
 
 static int
 ethsw_switchdev_port_attr_set_event(struct net_device *netdev,
-		struct switchdev_notifier_port_attr_info *port_attr_info)
+				    struct switchdev_notifier_port_attr_info
+				    *port_attr_info)
 {
 	int err;
 
@@ -1277,7 +1278,8 @@ err_addr_alloc:
 
 static int
 ethsw_switchdev_port_obj_event(unsigned long event, struct net_device *netdev,
-			struct switchdev_notifier_port_obj_info *port_obj_info)
+			       struct switchdev_notifier_port_obj_info
+			       *port_obj_info)
 {
 	int err = -EOPNOTSUPP;
 
diff --git a/drivers/staging/gasket/gasket_page_table.c b/drivers/staging/gasket/gasket_page_table.c
index f6d715787da8..f3dbe0fe2a67 100644
--- a/drivers/staging/gasket/gasket_page_table.c
+++ b/drivers/staging/gasket/gasket_page_table.c
@@ -898,7 +898,7 @@ static int gasket_alloc_extended_subtable(struct gasket_page_table *pg_tbl,
  *
  * Note that memory for second level page tables is allocated as needed, but
  * that memory is only freed on the final close	of the device file, when the
- * page tables are repartitioned, or the the device is removed.  If there is an
+ * page tables are repartitioned, or the device is removed.  If there is an
  * error or if the full range of slots is not available, any memory
  * allocated for second level page tables remains allocated until final close,
  * repartition, or device removal.
diff --git a/drivers/staging/gasket/gasket_sysfs.c b/drivers/staging/gasket/gasket_sysfs.c
index 5f0e089573a2..af26bc9f184a 100644
--- a/drivers/staging/gasket/gasket_sysfs.c
+++ b/drivers/staging/gasket/gasket_sysfs.c
@@ -339,6 +339,7 @@ void gasket_sysfs_put_attr(struct device *device,
 
 	dev_err(device, "Unable to put unknown attribute: %s\n",
 		attr->attr.attr.name);
+	put_mapping(mapping);
 }
 EXPORT_SYMBOL(gasket_sysfs_put_attr);
 
@@ -372,6 +373,7 @@ ssize_t gasket_sysfs_register_store(struct device *device,
 	gasket_dev = mapping->gasket_dev;
 	if (!gasket_dev) {
 		dev_err(device, "Device driver may have been removed\n");
+		put_mapping(mapping);
 		return 0;
 	}
 
diff --git a/drivers/staging/gdm724x/gdm_lte.c b/drivers/staging/gdm724x/gdm_lte.c
index 354727f0a1fc..eb309190f5be 100644
--- a/drivers/staging/gdm724x/gdm_lte.c
+++ b/drivers/staging/gdm724x/gdm_lte.c
@@ -172,7 +172,7 @@ static int gdm_lte_emulate_arp(struct sk_buff *skb_in, u32 nic_type)
 
 static __sum16 icmp6_checksum(struct ipv6hdr *ipv6, u16 *ptr, int len)
 {
-	unsigned short *w = ptr;
+	unsigned short *w;
 	__wsum sum = 0;
 	int i;
 	u16 pa;
diff --git a/drivers/staging/greybus/hid.c b/drivers/staging/greybus/hid.c
index 04bfd9110502..ed706f39e87a 100644
--- a/drivers/staging/greybus/hid.c
+++ b/drivers/staging/greybus/hid.c
@@ -290,9 +290,8 @@ static int gb_hid_parse(struct hid_device *hid)
 	}
 
 	rdesc = kzalloc(rsize, GFP_KERNEL);
-	if (!rdesc) {
+	if (!rdesc)
 		return -ENOMEM;
-	}
 
 	ret = gb_hid_get_report_desc(ghid, rdesc);
 	if (ret) {
diff --git a/drivers/staging/greybus/light.c b/drivers/staging/greybus/light.c
index d6ba25f21d80..d2672b65c3f4 100644
--- a/drivers/staging/greybus/light.c
+++ b/drivers/staging/greybus/light.c
@@ -1026,7 +1026,8 @@ static int gb_lights_light_config(struct gb_lights *glights, u8 id)
 
 	light->channels_count = conf.channel_count;
 	light->name = kstrndup(conf.name, NAMES_MAX, GFP_KERNEL);
-
+	if (!light->name)
+		return -ENOMEM;
 	light->channels = kcalloc(light->channels_count,
 				  sizeof(struct gb_channel), GFP_KERNEL);
 	if (!light->channels)
diff --git a/drivers/staging/greybus/loopback.c b/drivers/staging/greybus/loopback.c
index 583d9708a191..2471448ba42a 100644
--- a/drivers/staging/greybus/loopback.c
+++ b/drivers/staging/greybus/loopback.c
@@ -135,7 +135,7 @@ static ssize_t name##_##field##_show(struct device *dev,	\
 			    char *buf)					\
 {									\
 	struct gb_loopback *gb = dev_get_drvdata(dev);			\
-	/* Report 0 for min and max if no transfer successed */		\
+	/* Report 0 for min and max if no transfer succeeded */		\
 	if (!gb->requests_completed)					\
 		return sprintf(buf, "0\n");				\
 	return sprintf(buf, "%" #type "\n", gb->name.field);		\
diff --git a/drivers/staging/greybus/uart.c b/drivers/staging/greybus/uart.c
index 4ffb334cd5cd..607378bfebb7 100644
--- a/drivers/staging/greybus/uart.c
+++ b/drivers/staging/greybus/uart.c
@@ -40,14 +40,6 @@
 #define GB_UART_FIRMWARE_CREDITS	4096
 #define GB_UART_CREDIT_WAIT_TIMEOUT_MSEC	10000
 
-struct gb_tty_line_coding {
-	__le32	rate;
-	__u8	format;
-	__u8	parity;
-	__u8	data_bits;
-	__u8	flow_control;
-};
-
 struct gb_tty {
 	struct gbphy_device *gbphy_dev;
 	struct tty_port port;
@@ -66,7 +58,7 @@ struct gb_tty {
 	struct mutex mutex;
 	u8 ctrlin;	/* input control lines */
 	u8 ctrlout;	/* output control lines */
-	struct gb_tty_line_coding line_coding;
+	struct gb_uart_set_line_coding_request line_coding;
 	struct work_struct tx_work;
 	struct kfifo write_fifo;
 	bool close_pending;
@@ -288,12 +280,9 @@ static void  gb_uart_tx_write_work(struct work_struct *work)
 
 static int send_line_coding(struct gb_tty *tty)
 {
-	struct gb_uart_set_line_coding_request request;
-
-	memcpy(&request, &tty->line_coding,
-	       sizeof(tty->line_coding));
 	return gb_operation_sync(tty->connection, GB_UART_TYPE_SET_LINE_CODING,
-				 &request, sizeof(request), NULL, 0);
+				 &tty->line_coding, sizeof(tty->line_coding),
+				 NULL, 0);
 }
 
 static int send_control(struct gb_tty *gb_tty, u8 control)
@@ -493,9 +482,9 @@ static int gb_tty_break_ctl(struct tty_struct *tty, int state)
 static void gb_tty_set_termios(struct tty_struct *tty,
 			       struct ktermios *termios_old)
 {
+	struct gb_uart_set_line_coding_request newline;
 	struct gb_tty *gb_tty = tty->driver_data;
 	struct ktermios *termios = &tty->termios;
-	struct gb_tty_line_coding newline;
 	u8 newctrl = gb_tty->ctrlout;
 
 	newline.rate = cpu_to_le32(tty_get_baud_rate(tty));
diff --git a/drivers/staging/iio/Documentation/overview.txt b/drivers/staging/iio/Documentation/overview.txt
index 43f92b06bc3e..ebdc64f451d7 100644
--- a/drivers/staging/iio/Documentation/overview.txt
+++ b/drivers/staging/iio/Documentation/overview.txt
@@ -34,7 +34,7 @@ turned on or off (if possible) via sysfs interfaces.
 fifo / ring buffers on the sensor chip.  These greatly reduce the load
 on the host CPU by buffering relatively large numbers of data samples
 based on an internal sampling clock. Examples include VTI SCA3000
-series and Analog Device ADXL345 accelerometers.  Each buffer supports
+series and Analog Devices ADXL345 accelerometers.  Each buffer supports
 polling to establish when data is available.
 
 * Trigger and software buffer support. In many data analysis
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index af0bcf95ee8a..c468355b0848 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -602,11 +602,12 @@ static const struct iio_buffer_setup_ops ad5933_ring_setup_ops = {
 	.postdisable = ad5933_ring_postdisable,
 };
 
-static int ad5933_register_ring_funcs_and_init(struct iio_dev *indio_dev)
+static int ad5933_register_ring_funcs_and_init(struct device *dev,
+					       struct iio_dev *indio_dev)
 {
 	struct iio_buffer *buffer;
 
-	buffer = iio_kfifo_allocate();
+	buffer = devm_iio_kfifo_allocate(dev);
 	if (!buffer)
 		return -ENOMEM;
 
@@ -676,6 +677,20 @@ static void ad5933_work(struct work_struct *work)
 	}
 }
 
+static void ad5933_reg_disable(void *data)
+{
+	struct ad5933_state *st = data;
+
+	regulator_disable(st->reg);
+}
+
+static void ad5933_clk_disable(void *data)
+{
+	struct ad5933_state *st = data;
+
+	clk_disable_unprepare(st->mclk);
+}
+
 static int ad5933_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
@@ -703,23 +718,32 @@ static int ad5933_probe(struct i2c_client *client,
 		dev_err(&client->dev, "Failed to enable specified VDD supply\n");
 		return ret;
 	}
-	ret = regulator_get_voltage(st->reg);
 
+	ret = devm_add_action_or_reset(&client->dev, ad5933_reg_disable, st);
+	if (ret)
+		return ret;
+
+	ret = regulator_get_voltage(st->reg);
 	if (ret < 0)
-		goto error_disable_reg;
+		return ret;
 
 	st->vref_mv = ret / 1000;
 
 	st->mclk = devm_clk_get(&client->dev, "mclk");
-	if (IS_ERR(st->mclk) && PTR_ERR(st->mclk) != -ENOENT) {
-		ret = PTR_ERR(st->mclk);
-		goto error_disable_reg;
-	}
+	if (IS_ERR(st->mclk) && PTR_ERR(st->mclk) != -ENOENT)
+		return PTR_ERR(st->mclk);
 
 	if (!IS_ERR(st->mclk)) {
 		ret = clk_prepare_enable(st->mclk);
 		if (ret < 0)
-			goto error_disable_reg;
+			return ret;
+
+		ret = devm_add_action_or_reset(&client->dev,
+					       ad5933_clk_disable,
+					       st);
+		if (ret)
+			return ret;
+
 		ext_clk_hz = clk_get_rate(st->mclk);
 	}
 
@@ -742,41 +766,15 @@ static int ad5933_probe(struct i2c_client *client,
 	indio_dev->channels = ad5933_channels;
 	indio_dev->num_channels = ARRAY_SIZE(ad5933_channels);
 
-	ret = ad5933_register_ring_funcs_and_init(indio_dev);
+	ret = ad5933_register_ring_funcs_and_init(&client->dev, indio_dev);
 	if (ret)
-		goto error_disable_mclk;
+		return ret;
 
 	ret = ad5933_setup(st);
 	if (ret)
-		goto error_unreg_ring;
-
-	ret = iio_device_register(indio_dev);
-	if (ret)
-		goto error_unreg_ring;
-
-	return 0;
-
-error_unreg_ring:
-	iio_kfifo_free(indio_dev->buffer);
-error_disable_mclk:
-	clk_disable_unprepare(st->mclk);
-error_disable_reg:
-	regulator_disable(st->reg);
-
-	return ret;
-}
-
-static int ad5933_remove(struct i2c_client *client)
-{
-	struct iio_dev *indio_dev = i2c_get_clientdata(client);
-	struct ad5933_state *st = iio_priv(indio_dev);
-
-	iio_device_unregister(indio_dev);
-	iio_kfifo_free(indio_dev->buffer);
-	regulator_disable(st->reg);
-	clk_disable_unprepare(st->mclk);
+		return ret;
 
-	return 0;
+	return devm_iio_device_register(&client->dev, indio_dev);
 }
 
 static const struct i2c_device_id ad5933_id[] = {
@@ -801,7 +799,6 @@ static struct i2c_driver ad5933_driver = {
 		.of_match_table = ad5933_of_match,
 	},
 	.probe = ad5933_probe,
-	.remove = ad5933_remove,
 	.id_table = ad5933_id,
 };
 module_i2c_driver(ad5933_driver);
diff --git a/drivers/staging/most/usb/Kconfig b/drivers/staging/most/usb/Kconfig
index a86f1f63def4..75dc25c0e0e5 100644
--- a/drivers/staging/most/usb/Kconfig
+++ b/drivers/staging/most/usb/Kconfig
@@ -7,7 +7,7 @@ config MOST_USB
 	tristate "USB"
 	depends on USB && NET
 	help
-	  Say Y here if you want to connect via USB to network tranceiver.
+	  Say Y here if you want to connect via USB to network transceiver.
 	  This device driver depends on the networking AIM.
 
 	  To compile this driver as a module, choose M here: the
diff --git a/drivers/staging/most/usb/usb.c b/drivers/staging/most/usb/usb.c
index e8c5a8c98375..2640c5b326a4 100644
--- a/drivers/staging/most/usb/usb.c
+++ b/drivers/staging/most/usb/usb.c
@@ -5,7 +5,6 @@
  * Copyright (C) 2013-2015 Microchip Technology Germany II GmbH & Co. KG
  */
 
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 #include <linux/module.h>
 #include <linux/fs.h>
 #include <linux/usb.h>
@@ -140,9 +139,10 @@ static void wq_netinfo(struct work_struct *wq_obj);
 static inline int drci_rd_reg(struct usb_device *dev, u16 reg, u16 *buf)
 {
 	int retval;
-	__le16 *dma_buf = kzalloc(sizeof(*dma_buf), GFP_KERNEL);
+	__le16 *dma_buf;
 	u8 req_type = USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_DEVICE;
 
+	dma_buf = kzalloc(sizeof(*dma_buf), GFP_KERNEL);
 	if (!dma_buf)
 		return -ENOMEM;
 
@@ -153,7 +153,9 @@ static inline int drci_rd_reg(struct usb_device *dev, u16 reg, u16 *buf)
 	*buf = le16_to_cpu(*dma_buf);
 	kfree(dma_buf);
 
-	return retval;
+	if (retval < 0)
+		return retval;
+	return 0;
 }
 
 /**
@@ -184,16 +186,18 @@ static inline int start_sync_ep(struct usb_device *usb_dev, u16 ep)
 
 /**
  * get_stream_frame_size - calculate frame size of current configuration
+ * @dev: device structure
  * @cfg: channel configuration
  */
-static unsigned int get_stream_frame_size(struct most_channel_config *cfg)
+static unsigned int get_stream_frame_size(struct device *dev,
+					  struct most_channel_config *cfg)
 {
-	unsigned int frame_size = 0;
+	unsigned int frame_size;
 	unsigned int sub_size = cfg->subbuffer_size;
 
 	if (!sub_size) {
-		pr_warn("Misconfig: Subbuffer size zero.\n");
-		return frame_size;
+		dev_warn(dev, "Misconfig: Subbuffer size zero.\n");
+		return 0;
 	}
 	switch (cfg->data_type) {
 	case MOST_CH_ISOC:
@@ -201,7 +205,7 @@ static unsigned int get_stream_frame_size(struct most_channel_config *cfg)
 		break;
 	case MOST_CH_SYNC:
 		if (cfg->packets_per_xact == 0) {
-			pr_warn("Misconfig: Packets per XACT zero\n");
+			dev_warn(dev, "Misconfig: Packets per XACT zero\n");
 			frame_size = 0;
 		} else if (cfg->packets_per_xact == 0xFF) {
 			frame_size = (USB_MTU / sub_size) * sub_size;
@@ -210,7 +214,8 @@ static unsigned int get_stream_frame_size(struct most_channel_config *cfg)
 		}
 		break;
 	default:
-		pr_warn("Query frame size of non-streaming channel\n");
+		dev_warn(dev, "Query frame size of non-streaming channel\n");
+		frame_size = 0;
 		break;
 	}
 	return frame_size;
@@ -233,11 +238,7 @@ static int hdm_poison_channel(struct most_interface *iface, int channel)
 	unsigned long flags;
 	spinlock_t *lock; /* temp. lock */
 
-	if (unlikely(!iface)) {
-		dev_warn(&mdev->usb_device->dev, "Poison: Bad interface.\n");
-		return -EIO;
-	}
-	if (unlikely(channel < 0 || channel >= iface->num_channels)) {
+	if (channel < 0 || channel >= iface->num_channels) {
 		dev_warn(&mdev->usb_device->dev, "Channel ID out of range.\n");
 		return -ECHRNG;
 	}
@@ -274,17 +275,17 @@ static int hdm_poison_channel(struct most_interface *iface, int channel)
 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 frame_size = get_stream_frame_size(&mdev->dev, conf);
 	unsigned int j, num_frames;
 
 	if (!frame_size)
-		return -EIO;
+		return -EINVAL;
 	num_frames = mbo->buffer_length / frame_size;
 
 	if (num_frames < 1) {
 		dev_err(&mdev->usb_device->dev,
 			"Missed minimal transfer unit.\n");
-		return -EIO;
+		return -EINVAL;
 	}
 
 	for (j = num_frames - 1; j > 0; j--)
@@ -308,11 +309,11 @@ static int hdm_remove_padding(struct most_dev *mdev, int channel,
 			      struct mbo *mbo)
 {
 	struct most_channel_config *const conf = &mdev->conf[channel];
-	unsigned int frame_size = get_stream_frame_size(conf);
+	unsigned int frame_size = get_stream_frame_size(&mdev->dev, conf);
 	unsigned int j, num_frames;
 
 	if (!frame_size)
-		return -EIO;
+		return -EINVAL;
 	num_frames = mbo->processed_length / USB_MTU;
 
 	for (j = 1; j < num_frames; j++)
@@ -386,103 +387,6 @@ static void hdm_write_completion(struct urb *urb)
  * padding bytes -if necessary- and calls the completion function.
  *
  * Context: interrupt!
- *
- * **************************************************************************
- *                   Error codes returned by in urb->status
- *                   or in iso_frame_desc[n].status (for ISO)
- * *************************************************************************
- *
- * USB device drivers may only test urb status values in completion handlers.
- * This is because otherwise there would be a race between HCDs updating
- * these values on one CPU, and device drivers testing them on another CPU.
- *
- * A transfer's actual_length may be positive even when an error has been
- * reported.  That's because transfers often involve several packets, so that
- * one or more packets could finish before an error stops further endpoint I/O.
- *
- * For isochronous URBs, the urb status value is non-zero only if the URB is
- * unlinked, the device is removed, the host controller is disabled or the total
- * transferred length is less than the requested length and the URB_SHORT_NOT_OK
- * flag is set.  Completion handlers for isochronous URBs should only see
- * urb->status set to zero, -ENOENT, -ECONNRESET, -ESHUTDOWN, or -EREMOTEIO.
- * Individual frame descriptor status fields may report more status codes.
- *
- *
- * 0			Transfer completed successfully
- *
- * -ENOENT		URB was synchronously unlinked by usb_unlink_urb
- *
- * -EINPROGRESS		URB still pending, no results yet
- *			(That is, if drivers see this it's a bug.)
- *
- * -EPROTO (*, **)	a) bitstuff error
- *			b) no response packet received within the
- *			   prescribed bus turn-around time
- *			c) unknown USB error
- *
- * -EILSEQ (*, **)	a) CRC mismatch
- *			b) no response packet received within the
- *			   prescribed bus turn-around time
- *			c) unknown USB error
- *
- *			Note that often the controller hardware does not
- *			distinguish among cases a), b), and c), so a
- *			driver cannot tell whether there was a protocol
- *			error, a failure to respond (often caused by
- *			device disconnect), or some other fault.
- *
- * -ETIME (**)		No response packet received within the prescribed
- *			bus turn-around time.  This error may instead be
- *			reported as -EPROTO or -EILSEQ.
- *
- * -ETIMEDOUT		Synchronous USB message functions use this code
- *			to indicate timeout expired before the transfer
- *			completed, and no other error was reported by HC.
- *
- * -EPIPE (**)		Endpoint stalled.  For non-control endpoints,
- *			reset this status with usb_clear_halt().
- *
- * -ECOMM		During an IN transfer, the host controller
- *			received data from an endpoint faster than it
- *			could be written to system memory
- *
- * -ENOSR		During an OUT transfer, the host controller
- *			could not retrieve data from system memory fast
- *			enough to keep up with the USB data rate
- *
- * -EOVERFLOW (*)	The amount of data returned by the endpoint was
- *			greater than either the max packet size of the
- *			endpoint or the remaining buffer size.  "Babble".
- *
- * -EREMOTEIO		The data read from the endpoint did not fill the
- *			specified buffer, and URB_SHORT_NOT_OK was set in
- *			urb->transfer_flags.
- *
- * -ENODEV		Device was removed.  Often preceded by a burst of
- *			other errors, since the hub driver doesn't detect
- *			device removal events immediately.
- *
- * -EXDEV		ISO transfer only partially completed
- *			(only set in iso_frame_desc[n].status, not urb->status)
- *
- * -EINVAL		ISO madness, if this happens: Log off and go home
- *
- * -ECONNRESET		URB was asynchronously unlinked by usb_unlink_urb
- *
- * -ESHUTDOWN		The device or host controller has been disabled due
- *			to some problem that could not be worked around,
- *			such as a physical disconnect.
- *
- *
- * (*) Error codes like -EPROTO, -EILSEQ and -EOVERFLOW normally indicate
- * hardware problems such as bad devices (including firmware) or cables.
- *
- * (**) This is also one of several codes that different kinds of host
- * controller use to indicate a transfer has failed because of device
- * disconnect.  In the interval before the hub driver starts disconnect
- * processing, devices may receive such fault reports for every request.
- *
- * See <https://www.kernel.org/doc/Documentation/driver-api/usb/error-codes.rst>
  */
 static void hdm_read_completion(struct urb *urb)
 {
@@ -552,36 +456,33 @@ static void hdm_read_completion(struct urb *urb)
 static int hdm_enqueue(struct most_interface *iface, int channel,
 		       struct mbo *mbo)
 {
-	struct most_dev *mdev;
+	struct most_dev *mdev = to_mdev(iface);
 	struct most_channel_config *conf;
 	int retval = 0;
 	struct urb *urb;
 	unsigned long length;
 	void *virt_address;
 
-	if (unlikely(!iface || !mbo))
-		return -EIO;
-	if (unlikely(iface->num_channels <= channel || channel < 0))
+	if (!mbo)
+		return -EINVAL;
+	if (iface->num_channels <= channel || channel < 0)
 		return -ECHRNG;
 
-	mdev = to_mdev(iface);
+	urb = usb_alloc_urb(NO_ISOCHRONOUS_URB, GFP_KERNEL);
+	if (!urb)
+		return -ENOMEM;
+
 	conf = &mdev->conf[channel];
 
 	mutex_lock(&mdev->io_mutex);
 	if (!mdev->usb_device) {
 		retval = -ENODEV;
-		goto unlock_io_mutex;
-	}
-
-	urb = usb_alloc_urb(NO_ISOCHRONOUS_URB, GFP_ATOMIC);
-	if (!urb) {
-		retval = -ENOMEM;
-		goto unlock_io_mutex;
+		goto err_free_urb;
 	}
 
 	if ((conf->direction & MOST_CH_TX) && mdev->padding_active[channel] &&
 	    hdm_add_padding(mdev, channel, mbo)) {
-		retval = -EIO;
+		retval = -EINVAL;
 		goto err_free_urb;
 	}
 
@@ -619,13 +520,13 @@ static int hdm_enqueue(struct most_interface *iface, int channel,
 			"URB submit failed with error %d.\n", retval);
 		goto err_unanchor_urb;
 	}
-	goto unlock_io_mutex;
+	mutex_unlock(&mdev->io_mutex);
+	return 0;
 
 err_unanchor_urb:
 	usb_unanchor_urb(urb);
 err_free_urb:
 	usb_free_urb(urb);
-unlock_io_mutex:
 	mutex_unlock(&mdev->io_mutex);
 	return retval;
 }
@@ -669,19 +570,20 @@ static int hdm_configure_channel(struct most_interface *iface, int channel,
 	struct most_dev *mdev = to_mdev(iface);
 	struct device *dev = &mdev->usb_device->dev;
 
-	mdev->is_channel_healthy[channel] = true;
-	mdev->clear_work[channel].channel = channel;
-	mdev->clear_work[channel].mdev = mdev;
-	INIT_WORK(&mdev->clear_work[channel].ws, wq_clear_halt);
-
-	if (unlikely(!iface || !conf)) {
-		dev_err(dev, "Bad interface or config pointer.\n");
+	if (!conf) {
+		dev_err(dev, "Bad config pointer.\n");
 		return -EINVAL;
 	}
-	if (unlikely(channel < 0 || channel >= iface->num_channels)) {
+	if (channel < 0 || channel >= iface->num_channels) {
 		dev_err(dev, "Channel ID out of range.\n");
 		return -EINVAL;
 	}
+
+	mdev->is_channel_healthy[channel] = true;
+	mdev->clear_work[channel].channel = channel;
+	mdev->clear_work[channel].mdev = mdev;
+	INIT_WORK(&mdev->clear_work[channel].ws, wq_clear_halt);
+
 	if (!conf->num_buffers || !conf->buffer_size) {
 		dev_err(dev, "Misconfig: buffer size or #buffers zero.\n");
 		return -EINVAL;
@@ -701,7 +603,7 @@ static int hdm_configure_channel(struct most_interface *iface, int channel,
 
 	mdev->padding_active[channel] = true;
 
-	frame_size = get_stream_frame_size(conf);
+	frame_size = get_stream_frame_size(&mdev->dev, conf);
 	if (frame_size == 0 || frame_size > USB_MTU) {
 		dev_warn(dev, "Misconfig: frame size wrong\n");
 		return -EINVAL;
@@ -745,10 +647,8 @@ static void hdm_request_netinfo(struct most_interface *iface, int channel,
 						   unsigned char,
 						   unsigned char *))
 {
-	struct most_dev *mdev;
+	struct most_dev *mdev = to_mdev(iface);
 
-	BUG_ON(!iface);
-	mdev = to_mdev(iface);
 	mdev->on_netinfo = on_netinfo;
 	if (!on_netinfo)
 		return;
@@ -787,22 +687,22 @@ static void wq_netinfo(struct work_struct *wq_obj)
 	u16 hi, mi, lo, link;
 	u8 hw_addr[6];
 
-	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_HI, &hi) < 0) {
+	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_HI, &hi)) {
 		dev_err(dev, "Vendor request 'hw_addr_hi' failed\n");
 		return;
 	}
 
-	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_MI, &mi) < 0) {
+	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_MI, &mi)) {
 		dev_err(dev, "Vendor request 'hw_addr_mid' failed\n");
 		return;
 	}
 
-	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_LO, &lo) < 0) {
+	if (drci_rd_reg(usb_device, DRCI_REG_HW_ADDR_LO, &lo)) {
 		dev_err(dev, "Vendor request 'hw_addr_low' failed\n");
 		return;
 	}
 
-	if (drci_rd_reg(usb_device, DRCI_REG_NI_STATE, &link) < 0) {
+	if (drci_rd_reg(usb_device, DRCI_REG_NI_STATE, &link)) {
 		dev_err(dev, "Vendor request 'link status' failed\n");
 		return;
 	}
@@ -830,6 +730,8 @@ static void wq_clear_halt(struct work_struct *wq_obj)
 	struct most_dev *mdev = clear_work->mdev;
 	unsigned int channel = clear_work->channel;
 	int pipe = clear_work->pipe;
+	int snd_pipe;
+	int peer;
 
 	mutex_lock(&mdev->io_mutex);
 	most_stop_enqueue(&mdev->iface, channel);
@@ -847,9 +749,12 @@ static void wq_clear_halt(struct work_struct *wq_obj)
 	 */
 	if (mdev->conf[channel].data_type == MOST_CH_ASYNC &&
 	    mdev->conf[channel].direction == MOST_CH_RX) {
-		int peer = 1 - channel;
-		int snd_pipe = usb_sndbulkpipe(mdev->usb_device,
-					       mdev->ep_address[peer]);
+		if (channel == 0)
+			peer = 1;
+		else
+			peer = 0;
+		snd_pipe = usb_sndbulkpipe(mdev->usb_device,
+					   mdev->ep_address[peer]);
 		usb_clear_halt(mdev->usb_device, snd_pipe);
 	}
 	mdev->is_channel_healthy[channel] = true;
@@ -904,12 +809,12 @@ static int get_stat_reg_addr(const struct regs *regs, int size,
 	int i;
 
 	for (i = 0; i < size; i++) {
-		if (!strcmp(name, regs[i].name)) {
+		if (sysfs_streq(name, regs[i].name)) {
 			*reg_addr = regs[i].reg;
 			return 0;
 		}
 	}
-	return -EFAULT;
+	return -EINVAL;
 }
 
 #define get_static_reg_addr(regs, name, reg_addr) \
@@ -924,14 +829,14 @@ static ssize_t value_show(struct device *dev, struct device_attribute *attr,
 	u16 reg_addr;
 	int err;
 
-	if (!strcmp(name, "arb_address"))
+	if (sysfs_streq(name, "arb_address"))
 		return snprintf(buf, PAGE_SIZE, "%04x\n", dci_obj->reg_addr);
 
-	if (!strcmp(name, "arb_value"))
+	if (sysfs_streq(name, "arb_value"))
 		reg_addr = dci_obj->reg_addr;
 	else if (get_static_reg_addr(ro_regs, name, &reg_addr) &&
 		 get_static_reg_addr(rw_regs, name, &reg_addr))
-		return -EFAULT;
+		return -EINVAL;
 
 	err = drci_rd_reg(dci_obj->usb_device, reg_addr, &val);
 	if (err < 0)
@@ -948,24 +853,25 @@ static ssize_t value_store(struct device *dev, struct device_attribute *attr,
 	const char *name = attr->attr.name;
 	struct most_dci_obj *dci_obj = to_dci_obj(dev);
 	struct usb_device *usb_dev = dci_obj->usb_device;
-	int err = kstrtou16(buf, 16, &val);
+	int err;
 
+	err = kstrtou16(buf, 16, &val);
 	if (err)
 		return err;
 
-	if (!strcmp(name, "arb_address")) {
+	if (sysfs_streq(name, "arb_address")) {
 		dci_obj->reg_addr = val;
 		return count;
 	}
 
-	if (!strcmp(name, "arb_value"))
+	if (sysfs_streq(name, "arb_value"))
 		err = drci_wr_reg(usb_dev, dci_obj->reg_addr, val);
-	else if (!strcmp(name, "sync_ep"))
+	else if (sysfs_streq(name, "sync_ep"))
 		err = start_sync_ep(usb_dev, val);
 	else if (!get_static_reg_addr(rw_regs, name, &reg_addr))
 		err = drci_wr_reg(usb_dev, reg_addr, val);
 	else
-		return -EFAULT;
+		return -EINVAL;
 
 	if (err < 0)
 		return err;
@@ -1008,19 +914,13 @@ static struct attribute *dci_attrs[] = {
 	NULL,
 };
 
-static struct attribute_group dci_attr_group = {
-	.attrs = dci_attrs,
-};
-
-static const struct attribute_group *dci_attr_groups[] = {
-	&dci_attr_group,
-	NULL,
-};
+ATTRIBUTE_GROUPS(dci);
 
 static void release_dci(struct device *dev)
 {
 	struct most_dci_obj *dci = to_dci_obj(dev);
 
+	put_device(dev->parent);
 	kfree(dci);
 }
 
@@ -1048,18 +948,23 @@ hdm_probe(struct usb_interface *interface, const struct usb_device_id *id)
 	struct usb_host_interface *usb_iface_desc = interface->cur_altsetting;
 	struct usb_device *usb_dev = interface_to_usbdev(interface);
 	struct device *dev = &usb_dev->dev;
-	struct most_dev *mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
+	struct most_dev *mdev;
 	unsigned int i;
 	unsigned int num_endpoints;
 	struct most_channel_capability *tmp_cap;
 	struct usb_endpoint_descriptor *ep_desc;
-	int ret = 0;
+	int ret = -ENOMEM;
 
+	mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);
 	if (!mdev)
-		goto err_out_of_memory;
+		return -ENOMEM;
 
 	usb_set_intfdata(interface, mdev);
 	num_endpoints = usb_iface_desc->desc.bNumEndpoints;
+	if (num_endpoints > MAX_NUM_ENDPOINTS) {
+		kfree(mdev);
+		return -EINVAL;
+	}
 	mutex_init(&mdev->io_mutex);
 	INIT_WORK(&mdev->poll_work_obj, wq_netinfo);
 	timer_setup(&mdev->link_stat_timer, link_stat_timer_handler, 0);
@@ -1134,17 +1039,17 @@ hdm_probe(struct usb_interface *interface, const struct usb_device_id *id)
 		init_usb_anchor(&mdev->busy_urbs[i]);
 		spin_lock_init(&mdev->channel_lock[i]);
 	}
-	dev_notice(dev, "claimed gadget: Vendor=%4.4x ProdID=%4.4x Bus=%02x Device=%02x\n",
-		   le16_to_cpu(usb_dev->descriptor.idVendor),
-		   le16_to_cpu(usb_dev->descriptor.idProduct),
-		   usb_dev->bus->busnum,
-		   usb_dev->devnum);
-
-	dev_notice(dev, "device path: /sys/bus/usb/devices/%d-%s:%d.%d\n",
-		   usb_dev->bus->busnum,
-		   usb_dev->devpath,
-		   usb_dev->config->desc.bConfigurationValue,
-		   usb_iface_desc->desc.bInterfaceNumber);
+	dev_dbg(dev, "claimed gadget: Vendor=%4.4x ProdID=%4.4x Bus=%02x Device=%02x\n",
+		le16_to_cpu(usb_dev->descriptor.idVendor),
+		le16_to_cpu(usb_dev->descriptor.idProduct),
+		usb_dev->bus->busnum,
+		usb_dev->devnum);
+
+	dev_dbg(dev, "device path: /sys/bus/usb/devices/%d-%s:%d.%d\n",
+		usb_dev->bus->busnum,
+		usb_dev->devpath,
+		usb_dev->config->desc.bConfigurationValue,
+		usb_iface_desc->desc.bInterfaceNumber);
 
 	ret = most_register_interface(&mdev->iface);
 	if (ret)
@@ -1164,7 +1069,7 @@ hdm_probe(struct usb_interface *interface, const struct usb_device_id *id)
 
 		mdev->dci->dev.init_name = "dci";
 		mdev->dci->dev.parent = get_device(mdev->iface.dev);
-		mdev->dci->dev.groups = dci_attr_groups;
+		mdev->dci->dev.groups = dci_groups;
 		mdev->dci->dev.release = release_dci;
 		if (device_register(&mdev->dci->dev)) {
 			mutex_unlock(&mdev->io_mutex);
@@ -1188,11 +1093,6 @@ err_free_conf:
 	kfree(mdev->conf);
 err_free_mdev:
 	put_device(&mdev->dev);
-err_out_of_memory:
-	if (ret == 0 || ret == -ENOMEM) {
-		ret = -ENOMEM;
-		dev_err(dev, "out of memory\n");
-	}
 	return ret;
 }
 
@@ -1225,14 +1125,43 @@ static void hdm_disconnect(struct usb_interface *interface)
 	kfree(mdev->cap);
 	kfree(mdev->conf);
 	kfree(mdev->ep_address);
+	put_device(&mdev->dci->dev);
 	put_device(&mdev->dev);
 }
 
+static int hdm_suspend(struct usb_interface *interface, pm_message_t message)
+{
+	struct most_dev *mdev = usb_get_intfdata(interface);
+	int i;
+
+	mutex_lock(&mdev->io_mutex);
+	for (i = 0; i < mdev->iface.num_channels; i++) {
+		most_stop_enqueue(&mdev->iface, i);
+		usb_kill_anchored_urbs(&mdev->busy_urbs[i]);
+	}
+	mutex_unlock(&mdev->io_mutex);
+	return 0;
+}
+
+static int hdm_resume(struct usb_interface *interface)
+{
+	struct most_dev *mdev = usb_get_intfdata(interface);
+	int i;
+
+	mutex_lock(&mdev->io_mutex);
+	for (i = 0; i < mdev->iface.num_channels; i++)
+		most_resume_enqueue(&mdev->iface, i);
+	mutex_unlock(&mdev->io_mutex);
+	return 0;
+}
+
 static struct usb_driver hdm_usb = {
 	.name = "hdm_usb",
 	.id_table = usbid,
 	.probe = hdm_probe,
 	.disconnect = hdm_disconnect,
+	.resume = hdm_resume,
+	.suspend = hdm_suspend,
 };
 
 module_usb_driver(hdm_usb);
diff --git a/drivers/staging/mt7621-dts/mt7621.dtsi b/drivers/staging/mt7621-dts/mt7621.dtsi
index 9e5cf68731bb..82aa93634eda 100644
--- a/drivers/staging/mt7621-dts/mt7621.dtsi
+++ b/drivers/staging/mt7621-dts/mt7621.dtsi
@@ -523,11 +523,10 @@
 			0x01000000 0 0x00000000 0x1e160000 0 0x00010000 /* io space */
 		>;
 
-		#interrupt-cells = <1>;
-		interrupt-map-mask = <0xF0000 0 0 1>;
-		interrupt-map = <0x10000 0 0 1 &gic GIC_SHARED 4 IRQ_TYPE_LEVEL_HIGH>,
-				<0x20000 0 0 1 &gic GIC_SHARED 24 IRQ_TYPE_LEVEL_HIGH>,
-				<0x30000 0 0 1 &gic GIC_SHARED 25 IRQ_TYPE_LEVEL_HIGH>;
+		interrupt-parent = <&gic>;
+		interrupts = <GIC_SHARED 4 IRQ_TYPE_LEVEL_HIGH
+				GIC_SHARED 24 IRQ_TYPE_LEVEL_HIGH
+				GIC_SHARED 25 IRQ_TYPE_LEVEL_HIGH>;
 
 		status = "disabled";
 
diff --git a/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.txt b/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.txt
deleted file mode 100644
index a369d715378b..000000000000
--- a/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.txt
+++ /dev/null
@@ -1,28 +0,0 @@
-Mediatek Mt7621 PCIe PHY
-
-Required properties:
-- compatible: must be "mediatek,mt7621-pci-phy"
-- reg: base address and length of the PCIe PHY block
-- #phy-cells: must be <1> for pcie0_phy and for pcie1_phy.
-
-Example:
-	pcie0_phy: pcie-phy@1e149000 {
-		compatible = "mediatek,mt7621-pci-phy";
-		reg = <0x1e149000 0x0700>;
-		#phy-cells = <1>;
-	};
-
-	pcie1_phy: pcie-phy@1e14a000 {
-		compatible = "mediatek,mt7621-pci-phy";
-		reg = <0x1e14a000 0x0700>;
-		#phy-cells = <1>;
-	};
-
-	/* users of the PCIe phy */
-
-	pcie: pcie@1e140000 {
-		...
-		...
-		phys = <&pcie0_phy 0>, <&pcie0_phy 1>, <&pcie1_phy 0>;
-		phy-names = "pcie-phy0", "pcie-phy1", "pcie-phy2";
-	};
diff --git a/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.yaml b/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.yaml
new file mode 100644
index 000000000000..cf32bbc45b5d
--- /dev/null
+++ b/drivers/staging/mt7621-pci-phy/mediatek,mt7621-pci-phy.yaml
@@ -0,0 +1,36 @@
+# SPDX-License-Identifier: GPL-2.0
+%YAML 1.2
+---
+$id: "http://devicetree.org/schemas/phy/mediatek,mt7621-pci-phy.yaml#"
+$schema: "http://devicetree.org/meta-schemas/core.yaml#"
+
+title: Mediatek Mt7621 PCIe PHY Device Tree Bindings
+
+maintainers:
+  - Sergio Paracuellos <sergio.paracuellos@gmail.com>
+
+properties:
+  compatible:
+    const: mediatek,mt7621-pci-phy
+
+  reg:
+    maxItems: 1
+
+  "#phy-cells":
+    const: 1
+    description: selects if the phy is dual-ported
+
+required:
+  - compatible
+  - reg
+  - "#phy-cells"
+
+additionalProperties: false
+
+examples:
+  - |
+    pcie0_phy: pcie-phy@1e149000 {
+      compatible = "mediatek,mt7621-pci-phy";
+      reg = <0x1e149000 0x0700>;
+      #phy-cells = <1>;
+    };
diff --git a/drivers/staging/mt7621-pci/pci-mt7621.c b/drivers/staging/mt7621-pci/pci-mt7621.c
index f58e3a51fc71..f961b353c22e 100644
--- a/drivers/staging/mt7621-pci/pci-mt7621.c
+++ b/drivers/staging/mt7621-pci/pci-mt7621.c
@@ -55,7 +55,7 @@
 #define RALINK_PCI_IOBASE		0x002C
 
 /* PCICFG virtual bridges */
-#define PCIE_P2P_MAX			3
+#define PCIE_P2P_CNT			3
 #define PCIE_P2P_BR_DEVNUM_SHIFT(p)	(16 + (p) * 4)
 #define PCIE_P2P_BR_DEVNUM0_SHIFT	PCIE_P2P_BR_DEVNUM_SHIFT(0)
 #define PCIE_P2P_BR_DEVNUM1_SHIFT	PCIE_P2P_BR_DEVNUM_SHIFT(1)
@@ -97,6 +97,7 @@
  * @pcie_rst: pointer to port reset control
  * @gpio_rst: gpio reset
  * @slot: port slot
+ * @irq: GIC irq
  * @enabled: indicates if port is enabled
  */
 struct mt7621_pcie_port {
@@ -107,6 +108,7 @@ struct mt7621_pcie_port {
 	struct reset_control *pcie_rst;
 	struct gpio_desc *gpio_rst;
 	u32 slot;
+	int irq;
 	bool enabled;
 };
 
@@ -120,6 +122,7 @@ struct mt7621_pcie_port {
  * @dev: Pointer to PCIe device
  * @io_map_base: virtual memory base address for io
  * @ports: pointer to PCIe port information
+ * @irq_map: irq mapping info according pcie link status
  * @resets_inverted: depends on chip revision
  * reset lines are inverted.
  */
@@ -135,6 +138,7 @@ struct mt7621_pcie {
 	} offset;
 	unsigned long io_map_base;
 	struct list_head ports;
+	int irq_map[PCIE_P2P_CNT];
 	bool resets_inverted;
 };
 
@@ -279,6 +283,16 @@ static void setup_cm_memory_region(struct mt7621_pcie *pcie)
 	}
 }
 
+static int mt7621_map_irq(const struct pci_dev *pdev, u8 slot, u8 pin)
+{
+	struct mt7621_pcie *pcie = pdev->bus->sysdata;
+	struct device *dev = pcie->dev;
+	int irq = pcie->irq_map[slot];
+
+	dev_info(dev, "bus=%d slot=%d irq=%d\n", pdev->bus->number, slot, irq);
+	return irq;
+}
+
 static int mt7621_pci_parse_request_of_pci_ranges(struct mt7621_pcie *pcie)
 {
 	struct device *dev = pcie->dev;
@@ -330,6 +344,7 @@ static int mt7621_pcie_parse_port(struct mt7621_pcie *pcie,
 {
 	struct mt7621_pcie_port *port;
 	struct device *dev = pcie->dev;
+	struct platform_device *pdev = to_platform_device(dev);
 	struct device_node *pnode = dev->of_node;
 	struct resource regs;
 	char name[10];
@@ -371,6 +386,12 @@ static int mt7621_pcie_parse_port(struct mt7621_pcie *pcie,
 	port->slot = slot;
 	port->pcie = pcie;
 
+	port->irq = platform_get_irq(pdev, slot);
+	if (port->irq < 0) {
+		dev_err(dev, "Failed to get IRQ for PCIe%d\n", slot);
+		return -ENXIO;
+	}
+
 	INIT_LIST_HEAD(&port->list);
 	list_add_tail(&port->list, &pcie->ports);
 
@@ -502,17 +523,25 @@ static void mt7621_pcie_init_ports(struct mt7621_pcie *pcie)
 
 	mt7621_pcie_reset_ep_deassert(pcie);
 
+	tmp = NULL;
 	list_for_each_entry(port, &pcie->ports, list) {
 		u32 slot = port->slot;
 
 		if (!mt7621_pcie_port_is_linkup(port)) {
 			dev_err(dev, "pcie%d no card, disable it (RST & CLK)\n",
 				slot);
-			if (slot != 1)
-				phy_power_off(port->phy);
 			mt7621_control_assert(port);
 			mt7621_pcie_port_clk_disable(port);
 			port->enabled = false;
+
+			if (slot == 0) {
+				tmp = port;
+				continue;
+			}
+
+			if (slot == 1 && tmp && !tmp->enabled)
+				phy_power_off(tmp->phy);
+
 		}
 	}
 }
@@ -576,14 +605,16 @@ static void mt7621_pcie_enable_ports(struct mt7621_pcie *pcie)
 static int mt7621_pcie_init_virtual_bridges(struct mt7621_pcie *pcie)
 {
 	u32 pcie_link_status = 0;
-	u32 n;
-	int i;
-	u32 p2p_br_devnum[PCIE_P2P_MAX];
+	u32 n = 0;
+	int i = 0;
+	u32 p2p_br_devnum[PCIE_P2P_CNT];
+	int irqs[PCIE_P2P_CNT];
 	struct mt7621_pcie_port *port;
 
 	list_for_each_entry(port, &pcie->ports, list) {
 		u32 slot = port->slot;
 
+		irqs[i++] = port->irq;
 		if (port->enabled)
 			pcie_link_status |= BIT(slot);
 	}
@@ -591,12 +622,16 @@ static int mt7621_pcie_init_virtual_bridges(struct mt7621_pcie *pcie)
 	if (pcie_link_status == 0)
 		return -1;
 
-	n = 0;
-	for (i = 0; i < PCIE_P2P_MAX; i++)
+	/*
+	 * Assign device numbers from zero to the enabled ports,
+	 * then assigning remaining device numbers to any disabled
+	 * ports.
+	 */
+	for (i = 0; i < PCIE_P2P_CNT; i++)
 		if (pcie_link_status & BIT(i))
 			p2p_br_devnum[i] = n++;
 
-	for (i = 0; i < PCIE_P2P_MAX; i++)
+	for (i = 0; i < PCIE_P2P_CNT; i++)
 		if ((pcie_link_status & BIT(i)) == 0)
 			p2p_br_devnum[i] = n++;
 
@@ -606,6 +641,15 @@ static int mt7621_pcie_init_virtual_bridges(struct mt7621_pcie *pcie)
 		 (p2p_br_devnum[1] << PCIE_P2P_BR_DEVNUM1_SHIFT) |
 		 (p2p_br_devnum[2] << PCIE_P2P_BR_DEVNUM2_SHIFT));
 
+	/* Assign IRQs */
+	n = 0;
+	for (i = 0; i < PCIE_P2P_CNT; i++)
+		if (pcie_link_status & BIT(i))
+			pcie->irq_map[n++] = irqs[i];
+
+	for (i = n; i < PCIE_P2P_CNT; i++)
+		pcie->irq_map[i] = -1;
+
 	return 0;
 }
 
@@ -630,7 +674,7 @@ static int mt7621_pcie_register_host(struct pci_host_bridge *host,
 	host->busnr = pcie->busn.start;
 	host->dev.parent = pcie->dev;
 	host->ops = &mt7621_pci_ops;
-	host->map_irq = of_irq_parse_and_map_pci;
+	host->map_irq = mt7621_map_irq;
 	host->swizzle_irq = pci_common_swizzle;
 	host->sysdata = pcie;
 
diff --git a/drivers/staging/mt7621-pinctrl/pinctrl-rt2880.c b/drivers/staging/mt7621-pinctrl/pinctrl-rt2880.c
index d0f06790d38f..caaf9e34f1ee 100644
--- a/drivers/staging/mt7621-pinctrl/pinctrl-rt2880.c
+++ b/drivers/staging/mt7621-pinctrl/pinctrl-rt2880.c
@@ -220,7 +220,7 @@ static int rt2880_pinmux_index(struct rt2880_priv *p)
 	/* allocate our function and group mapping index buffers */
 	f = p->func = devm_kcalloc(p->dev,
 				   p->func_count,
-				   sizeof(struct rt2880_pmx_func),
+				   sizeof(*p->func),
 				   GFP_KERNEL);
 	gpio_func.groups = devm_kcalloc(p->dev, p->group_count, sizeof(int),
 					GFP_KERNEL);
diff --git a/drivers/staging/pi433/pi433_if.c b/drivers/staging/pi433/pi433_if.c
index 313d22f6210f..c8d0c63fdd1d 100644
--- a/drivers/staging/pi433/pi433_if.c
+++ b/drivers/staging/pi433/pi433_if.c
@@ -1230,6 +1230,7 @@ static int pi433_probe(struct spi_device *spi)
 	device->cdev = cdev_alloc();
 	if (!device->cdev) {
 		dev_dbg(device->dev, "allocation of cdev failed");
+		retval = -ENOMEM;
 		goto cdev_failed;
 	}
 	device->cdev->owner = THIS_MODULE;
diff --git a/drivers/staging/qlge/qlge_dbg.c b/drivers/staging/qlge/qlge_dbg.c
index 1795533cbd3a..058889687907 100644
--- a/drivers/staging/qlge/qlge_dbg.c
+++ b/drivers/staging/qlge/qlge_dbg.c
@@ -1559,18 +1559,17 @@ void ql_dump_stat(struct ql_adapter *qdev)
 #ifdef QL_DEV_DUMP
 
 #define DUMP_QDEV_FIELD(qdev, type, field)		\
-	pr_err("qdev->%-24s = " type "\n", #field, (qdev)->(field))
+	pr_err("qdev->%-24s = " type "\n", #field, (qdev)->field)
 #define DUMP_QDEV_DMA_FIELD(qdev, field)		\
 	pr_err("qdev->%-24s = %llx\n", #field, (unsigned long long)qdev->field)
 #define DUMP_QDEV_ARRAY(qdev, type, array, index, field) \
 	pr_err("%s[%d].%s = " type "\n",		 \
-	       #array, index, #field, (qdev)->array[index].field);
+	       #array, index, #field, (qdev)->array[index].field)
 void ql_dump_qdev(struct ql_adapter *qdev)
 {
 	int i;
 
 	DUMP_QDEV_FIELD(qdev, "%lx", flags);
-	DUMP_QDEV_FIELD(qdev, "%p", vlgrp);
 	DUMP_QDEV_FIELD(qdev, "%p", pdev);
 	DUMP_QDEV_FIELD(qdev, "%p", ndev);
 	DUMP_QDEV_FIELD(qdev, "%d", chip_rev_id);
@@ -1758,8 +1757,6 @@ void ql_dump_rx_ring(struct rx_ring *rx_ring)
 	       rx_ring->lbq.prod_idx_db_reg);
 	pr_err("rx_ring->lbq.next_to_use = %d\n", rx_ring->lbq.next_to_use);
 	pr_err("rx_ring->lbq.next_to_clean = %d\n", rx_ring->lbq.next_to_clean);
-	pr_err("rx_ring->lbq_clean_idx = %d\n", rx_ring->lbq_clean_idx);
-	pr_err("rx_ring->lbq_free_cnt = %d\n", rx_ring->lbq_free_cnt);
 
 	pr_err("rx_ring->sbq.base = %p\n", rx_ring->sbq.base);
 	pr_err("rx_ring->sbq.base_dma = %llx\n",
diff --git a/drivers/staging/qlge/qlge_main.c b/drivers/staging/qlge/qlge_main.c
index c92820f07968..402edaeffe12 100644
--- a/drivers/staging/qlge/qlge_main.c
+++ b/drivers/staging/qlge/qlge_main.c
@@ -214,19 +214,20 @@ int ql_write_cfg(struct ql_adapter *qdev, void *ptr, int size, u32 bit,
 	u32 mask;
 	u32 value;
 
-	direction =
-	    (bit & (CFG_LRQ | CFG_LR | CFG_LCQ)) ? PCI_DMA_TODEVICE :
-	    PCI_DMA_FROMDEVICE;
+	if (bit & (CFG_LRQ | CFG_LR | CFG_LCQ))
+		direction = DMA_TO_DEVICE;
+	else
+		direction = DMA_FROM_DEVICE;
 
-	map = pci_map_single(qdev->pdev, ptr, size, direction);
-	if (pci_dma_mapping_error(qdev->pdev, map)) {
+	map = dma_map_single(&qdev->pdev->dev, ptr, size, direction);
+	if (dma_mapping_error(&qdev->pdev->dev, map)) {
 		netif_err(qdev, ifup, qdev->ndev, "Couldn't map DMA area.\n");
 		return -ENOMEM;
 	}
 
 	status = ql_sem_spinlock(qdev, SEM_ICB_MASK);
 	if (status)
-		return status;
+		goto lock_failed;
 
 	status = ql_wait_cfg(qdev, bit);
 	if (status) {
@@ -235,8 +236,8 @@ int ql_write_cfg(struct ql_adapter *qdev, void *ptr, int size, u32 bit,
 		goto exit;
 	}
 
-	ql_write32(qdev, ICB_L, (u32) map);
-	ql_write32(qdev, ICB_H, (u32) (map >> 32));
+	ql_write32(qdev, ICB_L, (u32)map);
+	ql_write32(qdev, ICB_H, (u32)(map >> 32));
 
 	mask = CFG_Q_MASK | (bit << 16);
 	value = bit | (q_id << CFG_Q_SHIFT);
@@ -248,7 +249,8 @@ int ql_write_cfg(struct ql_adapter *qdev, void *ptr, int size, u32 bit,
 	status = ql_wait_cfg(qdev, bit);
 exit:
 	ql_sem_unlock(qdev, SEM_ICB_MASK);	/* does flush too */
-	pci_unmap_single(qdev->pdev, map, size, direction);
+lock_failed:
+	dma_unmap_single(&qdev->pdev->dev, map, size, direction);
 	return status;
 }
 
@@ -261,52 +263,50 @@ int ql_get_mac_addr_reg(struct ql_adapter *qdev, u32 type, u16 index,
 
 	switch (type) {
 	case MAC_ADDR_TYPE_MULTI_MAC:
-	case MAC_ADDR_TYPE_CAM_MAC:
-		{
-			status =
-			    ql_wait_reg_rdy(qdev,
-					    MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) | /* offset */
+	case MAC_ADDR_TYPE_CAM_MAC: {
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | /* offset */
 				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
-				   MAC_ADDR_ADR | MAC_ADDR_RS | type); /* type */
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
-			if (status)
-				goto exit;
-			*value++ = ql_read32(qdev, MAC_ADDR_DATA);
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) | /* offset */
+				   MAC_ADDR_ADR | MAC_ADDR_RS |
+				   type); /* type */
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
+		if (status)
+			break;
+		*value++ = ql_read32(qdev, MAC_ADDR_DATA);
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | /* offset */
 				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
-				   MAC_ADDR_ADR | MAC_ADDR_RS | type); /* type */
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
+				   MAC_ADDR_ADR | MAC_ADDR_RS |
+				   type); /* type */
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MR, 0);
+		if (status)
+			break;
+		*value++ = ql_read32(qdev, MAC_ADDR_DATA);
+		if (type == MAC_ADDR_TYPE_CAM_MAC) {
+			status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX,
+						 MAC_ADDR_MW, 0);
 			if (status)
-				goto exit;
+				break;
+			ql_write32(qdev, MAC_ADDR_IDX,
+				   (offset++) | /* offset */
+					   (index
+					    << MAC_ADDR_IDX_SHIFT) | /* index */
+					   MAC_ADDR_ADR |
+					   MAC_ADDR_RS | type); /* type */
+			status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX,
+						 MAC_ADDR_MR, 0);
+			if (status)
+				break;
 			*value++ = ql_read32(qdev, MAC_ADDR_DATA);
-			if (type == MAC_ADDR_TYPE_CAM_MAC) {
-				status =
-				    ql_wait_reg_rdy(qdev,
-						    MAC_ADDR_IDX, MAC_ADDR_MW,
-						    0);
-				if (status)
-					goto exit;
-				ql_write32(qdev, MAC_ADDR_IDX, (offset++) | /* offset */
-					   (index << MAC_ADDR_IDX_SHIFT) | /* index */
-					   MAC_ADDR_ADR | MAC_ADDR_RS | type); /* type */
-				status =
-				    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX,
-						    MAC_ADDR_MR, 0);
-				if (status)
-					goto exit;
-				*value++ = ql_read32(qdev, MAC_ADDR_DATA);
-			}
-			break;
 		}
+		break;
+	}
 	case MAC_ADDR_TYPE_VLAN:
 	case MAC_ADDR_TYPE_MULTI_FLTR:
 	default:
@@ -314,7 +314,6 @@ int ql_get_mac_addr_reg(struct ql_adapter *qdev, u32 type, u16 index,
 			   "Address type %d not yet supported.\n", type);
 		status = -EPERM;
 	}
-exit:
 	return status;
 }
 
@@ -328,107 +327,93 @@ static int ql_set_mac_addr_reg(struct ql_adapter *qdev, u8 *addr, u32 type,
 	int status = 0;
 
 	switch (type) {
-	case MAC_ADDR_TYPE_MULTI_MAC:
-		{
-			u32 upper = (addr[0] << 8) | addr[1];
-			u32 lower = (addr[2] << 24) | (addr[3] << 16) |
-					(addr[4] << 8) | (addr[5]);
-
-			status =
-				ql_wait_reg_rdy(qdev,
-						MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) |
-				(index << MAC_ADDR_IDX_SHIFT) |
-				type | MAC_ADDR_E);
-			ql_write32(qdev, MAC_ADDR_DATA, lower);
-			status =
-				ql_wait_reg_rdy(qdev,
-						MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) |
-				(index << MAC_ADDR_IDX_SHIFT) |
-				type | MAC_ADDR_E);
+	case MAC_ADDR_TYPE_MULTI_MAC: {
+		u32 upper = (addr[0] << 8) | addr[1];
+		u32 lower = (addr[2] << 24) | (addr[3] << 16) | (addr[4] << 8) |
+			    (addr[5]);
 
-			ql_write32(qdev, MAC_ADDR_DATA, upper);
-			status =
-				ql_wait_reg_rdy(qdev,
-						MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
 			break;
-		}
-	case MAC_ADDR_TYPE_CAM_MAC:
-		{
-			u32 cam_output;
-			u32 upper = (addr[0] << 8) | addr[1];
-			u32 lower =
-			    (addr[2] << 24) | (addr[3] << 16) | (addr[4] << 8) |
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | (index << MAC_ADDR_IDX_SHIFT) | type |
+				   MAC_ADDR_E);
+		ql_write32(qdev, MAC_ADDR_DATA, lower);
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | (index << MAC_ADDR_IDX_SHIFT) | type |
+				   MAC_ADDR_E);
+
+		ql_write32(qdev, MAC_ADDR_DATA, upper);
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		break;
+	}
+	case MAC_ADDR_TYPE_CAM_MAC: {
+		u32 cam_output;
+		u32 upper = (addr[0] << 8) | addr[1];
+		u32 lower = (addr[2] << 24) | (addr[3] << 16) | (addr[4] << 8) |
 			    (addr[5]);
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) | /* offset */
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | /* offset */
 				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
-				   type);	/* type */
-			ql_write32(qdev, MAC_ADDR_DATA, lower);
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset++) | /* offset */
+				   type); /* type */
+		ql_write32(qdev, MAC_ADDR_DATA, lower);
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
+			break;
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset++) | /* offset */
 				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
-				   type);	/* type */
-			ql_write32(qdev, MAC_ADDR_DATA, upper);
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, (offset) |	/* offset */
-				   (index << MAC_ADDR_IDX_SHIFT) |	/* index */
-				   type);	/* type */
-			/* This field should also include the queue id
-			 * and possibly the function id.  Right now we hardcode
-			 * the route field to NIC core.
-			 */
-			cam_output = (CAM_OUT_ROUTE_NIC |
-				      (qdev->
-				       func << CAM_OUT_FUNC_SHIFT) |
-					(0 << CAM_OUT_CQ_ID_SHIFT));
-			if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)
-				cam_output |= CAM_OUT_RV;
-			/* route to NIC core */
-			ql_write32(qdev, MAC_ADDR_DATA, cam_output);
+				   type); /* type */
+		ql_write32(qdev, MAC_ADDR_DATA, upper);
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
 			break;
-		}
-	case MAC_ADDR_TYPE_VLAN:
-		{
-			u32 enable_bit = *((u32 *) &addr[0]);
-			/* For VLAN, the addr actually holds a bit that
-			 * either enables or disables the vlan id we are
-			 * addressing. It's either MAC_ADDR_E on or off.
-			 * That's bit-27 we're talking about.
-			 */
-			status =
-			    ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
-			if (status)
-				goto exit;
-			ql_write32(qdev, MAC_ADDR_IDX, offset |	/* offset */
-				   (index << MAC_ADDR_IDX_SHIFT) |	/* index */
-				   type |	/* type */
-				   enable_bit);	/* enable/disable */
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   (offset) | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type); /* type */
+		/* This field should also include the queue id
+		 * and possibly the function id.  Right now we hardcode
+		 * the route field to NIC core.
+		 */
+		cam_output = (CAM_OUT_ROUTE_NIC |
+			      (qdev->func << CAM_OUT_FUNC_SHIFT) |
+			      (0 << CAM_OUT_CQ_ID_SHIFT));
+		if (qdev->ndev->features & NETIF_F_HW_VLAN_CTAG_RX)
+			cam_output |= CAM_OUT_RV;
+		/* route to NIC core */
+		ql_write32(qdev, MAC_ADDR_DATA, cam_output);
+		break;
+	}
+	case MAC_ADDR_TYPE_VLAN: {
+		u32 enable_bit = *((u32 *)&addr[0]);
+		/* For VLAN, the addr actually holds a bit that
+		 * either enables or disables the vlan id we are
+		 * addressing. It's either MAC_ADDR_E on or off.
+		 * That's bit-27 we're talking about.
+		 */
+		status = ql_wait_reg_rdy(qdev, MAC_ADDR_IDX, MAC_ADDR_MW, 0);
+		if (status)
 			break;
-		}
+		ql_write32(qdev, MAC_ADDR_IDX,
+			   offset | /* offset */
+				   (index << MAC_ADDR_IDX_SHIFT) | /* index */
+				   type | /* type */
+				   enable_bit); /* enable/disable */
+		break;
+	}
 	case MAC_ADDR_TYPE_MULTI_FLTR:
 	default:
 		netif_crit(qdev, ifup, qdev->ndev,
 			   "Address type %d not yet supported.\n", type);
 		status = -EPERM;
 	}
-exit:
 	return status;
 }
 
@@ -455,7 +440,7 @@ static int ql_set_mac_addr(struct ql_adapter *qdev, int set)
 	status = ql_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
 	if (status)
 		return status;
-	status = ql_set_mac_addr_reg(qdev, (u8 *) addr,
+	status = ql_set_mac_addr_reg(qdev, (u8 *)addr,
 				     MAC_ADDR_TYPE_CAM_MAC,
 				     qdev->func * MAX_CQ);
 	ql_sem_unlock(qdev, SEM_MAC_ADDR_MASK);
@@ -857,7 +842,7 @@ int ql_read_xgmac_reg64(struct ql_adapter *qdev, u32 reg, u64 *data)
 	if (status)
 		goto exit;
 
-	*data = (u64) lo | ((u64) hi << 32);
+	*data = (u64)lo | ((u64)hi << 32);
 
 exit:
 	return status;
@@ -983,14 +968,14 @@ static struct qlge_bq_desc *ql_get_curr_lchunk(struct ql_adapter *qdev,
 {
 	struct qlge_bq_desc *lbq_desc = qlge_get_curr_buf(&rx_ring->lbq);
 
-	pci_dma_sync_single_for_cpu(qdev->pdev, lbq_desc->dma_addr,
-				    qdev->lbq_buf_size, PCI_DMA_FROMDEVICE);
+	dma_sync_single_for_cpu(&qdev->pdev->dev, lbq_desc->dma_addr,
+				qdev->lbq_buf_size, DMA_FROM_DEVICE);
 
 	if ((lbq_desc->p.pg_chunk.offset + qdev->lbq_buf_size) ==
 	    ql_lbq_block_size(qdev)) {
 		/* last chunk of the master page */
-		pci_unmap_page(qdev->pdev, lbq_desc->dma_addr,
-			       ql_lbq_block_size(qdev), PCI_DMA_FROMDEVICE);
+		dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
+			       ql_lbq_block_size(qdev), DMA_FROM_DEVICE);
 	}
 
 	return lbq_desc;
@@ -1036,10 +1021,10 @@ static int qlge_refill_sb(struct rx_ring *rx_ring,
 		return -ENOMEM;
 	skb_reserve(skb, QLGE_SB_PAD);
 
-	sbq_desc->dma_addr = pci_map_single(qdev->pdev, skb->data,
+	sbq_desc->dma_addr = dma_map_single(&qdev->pdev->dev, skb->data,
 					    SMALL_BUF_MAP_SIZE,
-					    PCI_DMA_FROMDEVICE);
-	if (pci_dma_mapping_error(qdev->pdev, sbq_desc->dma_addr)) {
+					    DMA_FROM_DEVICE);
+	if (dma_mapping_error(&qdev->pdev->dev, sbq_desc->dma_addr)) {
 		netif_err(qdev, ifup, qdev->ndev, "PCI mapping failed.\n");
 		dev_kfree_skb_any(skb);
 		return -EIO;
@@ -1064,10 +1049,10 @@ static int qlge_refill_lb(struct rx_ring *rx_ring,
 		page = alloc_pages(gfp | __GFP_COMP, qdev->lbq_buf_order);
 		if (unlikely(!page))
 			return -ENOMEM;
-		dma_addr = pci_map_page(qdev->pdev, page, 0,
+		dma_addr = dma_map_page(&qdev->pdev->dev, page, 0,
 					ql_lbq_block_size(qdev),
-					PCI_DMA_FROMDEVICE);
-		if (pci_dma_mapping_error(qdev->pdev, dma_addr)) {
+					DMA_FROM_DEVICE);
+		if (dma_mapping_error(&qdev->pdev->dev, dma_addr)) {
 			__free_pages(page, qdev->lbq_buf_order);
 			netif_err(qdev, drv, qdev->ndev,
 				  "PCI mapping failed.\n");
@@ -1224,20 +1209,20 @@ static void ql_unmap_send(struct ql_adapter *qdev,
 					     qdev->ndev,
 					     "unmapping OAL area.\n");
 			}
-			pci_unmap_single(qdev->pdev,
+			dma_unmap_single(&qdev->pdev->dev,
 					 dma_unmap_addr(&tx_ring_desc->map[i],
 							mapaddr),
 					 dma_unmap_len(&tx_ring_desc->map[i],
 						       maplen),
-					 PCI_DMA_TODEVICE);
+					 DMA_TO_DEVICE);
 		} else {
 			netif_printk(qdev, tx_done, KERN_DEBUG, qdev->ndev,
 				     "unmapping frag %d.\n", i);
-			pci_unmap_page(qdev->pdev,
+			dma_unmap_page(&qdev->pdev->dev,
 				       dma_unmap_addr(&tx_ring_desc->map[i],
 						      mapaddr),
 				       dma_unmap_len(&tx_ring_desc->map[i],
-						     maplen), PCI_DMA_TODEVICE);
+						     maplen), DMA_TO_DEVICE);
 		}
 	}
 
@@ -1263,9 +1248,9 @@ static int ql_map_send(struct ql_adapter *qdev,
 	/*
 	 * Map the skb buffer first.
 	 */
-	map = pci_map_single(qdev->pdev, skb->data, len, PCI_DMA_TODEVICE);
+	map = dma_map_single(&qdev->pdev->dev, skb->data, len, DMA_TO_DEVICE);
 
-	err = pci_dma_mapping_error(qdev->pdev, map);
+	err = dma_mapping_error(&qdev->pdev->dev, map);
 	if (err) {
 		netif_err(qdev, tx_queued, qdev->ndev,
 			  "PCI mapping failed with error: %d\n", err);
@@ -1310,10 +1295,10 @@ static int ql_map_send(struct ql_adapter *qdev,
 			 *      etc...
 			 */
 			/* Tack on the OAL in the eighth segment of IOCB. */
-			map = pci_map_single(qdev->pdev, &tx_ring_desc->oal,
+			map = dma_map_single(&qdev->pdev->dev, &tx_ring_desc->oal,
 					     sizeof(struct oal),
-					     PCI_DMA_TODEVICE);
-			err = pci_dma_mapping_error(qdev->pdev, map);
+					     DMA_TO_DEVICE);
+			err = dma_mapping_error(&qdev->pdev->dev, map);
 			if (err) {
 				netif_err(qdev, tx_queued, qdev->ndev,
 					  "PCI mapping outbound address list with error: %d\n",
@@ -1584,8 +1569,8 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 	}
 	skb_reserve(new_skb, NET_IP_ALIGN);
 
-	pci_dma_sync_single_for_cpu(qdev->pdev, sbq_desc->dma_addr,
-				    SMALL_BUF_MAP_SIZE, PCI_DMA_FROMDEVICE);
+	dma_sync_single_for_cpu(&qdev->pdev->dev, sbq_desc->dma_addr,
+				SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
 
 	skb_put_data(new_skb, skb->data, length);
 
@@ -1647,7 +1632,7 @@ static void ql_process_mac_rx_skb(struct ql_adapter *qdev,
 		} else if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_U) &&
 				(ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_V4)) {
 			/* Unfragmented ipv4 UDP frame. */
-			struct iphdr *iph = (struct iphdr *) skb->data;
+			struct iphdr *iph = (struct iphdr *)skb->data;
 
 			if (!(iph->frag_off &
 				htons(IP_MF|IP_OFFSET))) {
@@ -1707,8 +1692,8 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 		 * Headers fit nicely into a small buffer.
 		 */
 		sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
-		pci_unmap_single(qdev->pdev, sbq_desc->dma_addr,
-				 SMALL_BUF_MAP_SIZE, PCI_DMA_FROMDEVICE);
+		dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+				 SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
 		skb = sbq_desc->p.skb;
 		ql_realign_skb(skb, hdr_len);
 		skb_put(skb, hdr_len);
@@ -1737,10 +1722,10 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 			 * buffer.
 			 */
 			sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
-			pci_dma_sync_single_for_cpu(qdev->pdev,
-						    sbq_desc->dma_addr,
-						    SMALL_BUF_MAP_SIZE,
-						    PCI_DMA_FROMDEVICE);
+			dma_sync_single_for_cpu(&qdev->pdev->dev,
+						sbq_desc->dma_addr,
+						SMALL_BUF_MAP_SIZE,
+						DMA_FROM_DEVICE);
 			skb_put_data(skb, sbq_desc->p.skb->data, length);
 		} else {
 			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
@@ -1750,9 +1735,9 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 			skb = sbq_desc->p.skb;
 			ql_realign_skb(skb, length);
 			skb_put(skb, length);
-			pci_unmap_single(qdev->pdev, sbq_desc->dma_addr,
+			dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
 					 SMALL_BUF_MAP_SIZE,
-					 PCI_DMA_FROMDEVICE);
+					 DMA_FROM_DEVICE);
 			sbq_desc->p.skb = NULL;
 		}
 	} else if (ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_DL) {
@@ -1787,9 +1772,9 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 					     "No skb available, drop the packet.\n");
 				return NULL;
 			}
-			pci_unmap_page(qdev->pdev, lbq_desc->dma_addr,
+			dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
 				       qdev->lbq_buf_size,
-				       PCI_DMA_FROMDEVICE);
+				       DMA_FROM_DEVICE);
 			skb_reserve(skb, NET_IP_ALIGN);
 			netif_printk(qdev, rx_status, KERN_DEBUG, qdev->ndev,
 				     "%d bytes of headers and data in large. Chain page to new skb and pull tail.\n",
@@ -1820,8 +1805,8 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 		int size, i = 0;
 
 		sbq_desc = qlge_get_curr_buf(&rx_ring->sbq);
-		pci_unmap_single(qdev->pdev, sbq_desc->dma_addr,
-				 SMALL_BUF_MAP_SIZE, PCI_DMA_FROMDEVICE);
+		dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
+				 SMALL_BUF_MAP_SIZE, DMA_FROM_DEVICE);
 		if (!(ib_mac_rsp->flags4 & IB_MAC_IOCB_RSP_HS)) {
 			/*
 			 * This is an non TCP/UDP IP frame, so
@@ -1936,7 +1921,7 @@ static void ql_process_mac_split_rx_intr(struct ql_adapter *qdev,
 		} else if ((ib_mac_rsp->flags2 & IB_MAC_IOCB_RSP_U) &&
 				(ib_mac_rsp->flags3 & IB_MAC_IOCB_RSP_V4)) {
 		/* Unfragmented ipv4 UDP frame. */
-			struct iphdr *iph = (struct iphdr *) skb->data;
+			struct iphdr *iph = (struct iphdr *)skb->data;
 
 			if (!(iph->frag_off &
 				htons(IP_MF|IP_OFFSET))) {
@@ -2317,7 +2302,7 @@ static int __qlge_vlan_rx_add_vid(struct ql_adapter *qdev, u16 vid)
 	u32 enable_bit = MAC_ADDR_E;
 	int err;
 
-	err = ql_set_mac_addr_reg(qdev, (u8 *) &enable_bit,
+	err = ql_set_mac_addr_reg(qdev, (u8 *)&enable_bit,
 				  MAC_ADDR_TYPE_VLAN, vid);
 	if (err)
 		netif_err(qdev, ifup, qdev->ndev,
@@ -2348,7 +2333,7 @@ static int __qlge_vlan_rx_kill_vid(struct ql_adapter *qdev, u16 vid)
 	u32 enable_bit = 0;
 	int err;
 
-	err = ql_set_mac_addr_reg(qdev, (u8 *) &enable_bit,
+	err = ql_set_mac_addr_reg(qdev, (u8 *)&enable_bit,
 				  MAC_ADDR_TYPE_VLAN, vid);
 	if (err)
 		netif_err(qdev, ifup, qdev->ndev,
@@ -2489,7 +2474,7 @@ static int ql_tso(struct sk_buff *skb, struct ob_mac_tso_iocb_req *mac_iocb_ptr)
 
 		mac_iocb_ptr->opcode = OPCODE_OB_MAC_TSO_IOCB;
 		mac_iocb_ptr->flags3 |= OB_MAC_TSO_IOCB_IC;
-		mac_iocb_ptr->frame_len = cpu_to_le32((u32) skb->len);
+		mac_iocb_ptr->frame_len = cpu_to_le32((u32)skb->len);
 		mac_iocb_ptr->total_hdrs_len =
 		    cpu_to_le16(skb_transport_offset(skb) + tcp_hdrlen(skb));
 		mac_iocb_ptr->net_trans_offset =
@@ -2527,7 +2512,7 @@ static void ql_hw_csum_setup(struct sk_buff *skb,
 	__sum16 *check;
 
 	mac_iocb_ptr->opcode = OPCODE_OB_MAC_TSO_IOCB;
-	mac_iocb_ptr->frame_len = cpu_to_le32((u32) skb->len);
+	mac_iocb_ptr->frame_len = cpu_to_le32((u32)skb->len);
 	mac_iocb_ptr->net_trans_offset =
 		cpu_to_le16(skb_network_offset(skb) |
 		skb_transport_offset(skb) << OB_MAC_TRANSPORT_HDR_SHIFT);
@@ -2558,7 +2543,7 @@ static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
 	struct ql_adapter *qdev = netdev_priv(ndev);
 	int tso;
 	struct tx_ring *tx_ring;
-	u32 tx_ring_idx = (u32) skb->queue_mapping;
+	u32 tx_ring_idx = (u32)skb->queue_mapping;
 
 	tx_ring = &qdev->tx_ring[tx_ring_idx];
 
@@ -2585,7 +2570,7 @@ static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
 	mac_iocb_ptr->txq_idx = tx_ring_idx;
 	tx_ring_desc->skb = skb;
 
-	mac_iocb_ptr->frame_len = cpu_to_le16((u16) skb->len);
+	mac_iocb_ptr->frame_len = cpu_to_le16((u16)skb->len);
 
 	if (skb_vlan_tag_present(skb)) {
 		netif_printk(qdev, tx_queued, KERN_DEBUG, qdev->ndev,
@@ -2636,17 +2621,17 @@ static netdev_tx_t qlge_send(struct sk_buff *skb, struct net_device *ndev)
 static void ql_free_shadow_space(struct ql_adapter *qdev)
 {
 	if (qdev->rx_ring_shadow_reg_area) {
-		pci_free_consistent(qdev->pdev,
-				    PAGE_SIZE,
-				    qdev->rx_ring_shadow_reg_area,
-				    qdev->rx_ring_shadow_reg_dma);
+		dma_free_coherent(&qdev->pdev->dev,
+				  PAGE_SIZE,
+				  qdev->rx_ring_shadow_reg_area,
+				  qdev->rx_ring_shadow_reg_dma);
 		qdev->rx_ring_shadow_reg_area = NULL;
 	}
 	if (qdev->tx_ring_shadow_reg_area) {
-		pci_free_consistent(qdev->pdev,
-				    PAGE_SIZE,
-				    qdev->tx_ring_shadow_reg_area,
-				    qdev->tx_ring_shadow_reg_dma);
+		dma_free_coherent(&qdev->pdev->dev,
+				  PAGE_SIZE,
+				  qdev->tx_ring_shadow_reg_area,
+				  qdev->tx_ring_shadow_reg_dma);
 		qdev->tx_ring_shadow_reg_area = NULL;
 	}
 }
@@ -2654,8 +2639,8 @@ static void ql_free_shadow_space(struct ql_adapter *qdev)
 static int ql_alloc_shadow_space(struct ql_adapter *qdev)
 {
 	qdev->rx_ring_shadow_reg_area =
-		pci_zalloc_consistent(qdev->pdev, PAGE_SIZE,
-				      &qdev->rx_ring_shadow_reg_dma);
+		dma_alloc_coherent(&qdev->pdev->dev, PAGE_SIZE,
+				   &qdev->rx_ring_shadow_reg_dma, GFP_ATOMIC);
 	if (!qdev->rx_ring_shadow_reg_area) {
 		netif_err(qdev, ifup, qdev->ndev,
 			  "Allocation of RX shadow space failed.\n");
@@ -2663,8 +2648,8 @@ static int ql_alloc_shadow_space(struct ql_adapter *qdev)
 	}
 
 	qdev->tx_ring_shadow_reg_area =
-		pci_zalloc_consistent(qdev->pdev, PAGE_SIZE,
-				      &qdev->tx_ring_shadow_reg_dma);
+		dma_alloc_coherent(&qdev->pdev->dev, PAGE_SIZE,
+				   &qdev->tx_ring_shadow_reg_dma, GFP_ATOMIC);
 	if (!qdev->tx_ring_shadow_reg_area) {
 		netif_err(qdev, ifup, qdev->ndev,
 			  "Allocation of TX shadow space failed.\n");
@@ -2673,10 +2658,10 @@ static int ql_alloc_shadow_space(struct ql_adapter *qdev)
 	return 0;
 
 err_wqp_sh_area:
-	pci_free_consistent(qdev->pdev,
-			    PAGE_SIZE,
-			    qdev->rx_ring_shadow_reg_area,
-			    qdev->rx_ring_shadow_reg_dma);
+	dma_free_coherent(&qdev->pdev->dev,
+			  PAGE_SIZE,
+			  qdev->rx_ring_shadow_reg_area,
+			  qdev->rx_ring_shadow_reg_dma);
 	return -ENOMEM;
 }
 
@@ -2702,8 +2687,8 @@ static void ql_free_tx_resources(struct ql_adapter *qdev,
 				 struct tx_ring *tx_ring)
 {
 	if (tx_ring->wq_base) {
-		pci_free_consistent(qdev->pdev, tx_ring->wq_size,
-				    tx_ring->wq_base, tx_ring->wq_base_dma);
+		dma_free_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+				  tx_ring->wq_base, tx_ring->wq_base_dma);
 		tx_ring->wq_base = NULL;
 	}
 	kfree(tx_ring->q);
@@ -2714,8 +2699,8 @@ static int ql_alloc_tx_resources(struct ql_adapter *qdev,
 				 struct tx_ring *tx_ring)
 {
 	tx_ring->wq_base =
-	    pci_alloc_consistent(qdev->pdev, tx_ring->wq_size,
-				 &tx_ring->wq_base_dma);
+	    dma_alloc_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+			       &tx_ring->wq_base_dma, GFP_ATOMIC);
 
 	if (!tx_ring->wq_base ||
 	    tx_ring->wq_base_dma & WQ_ADDR_ALIGN)
@@ -2729,8 +2714,8 @@ static int ql_alloc_tx_resources(struct ql_adapter *qdev,
 
 	return 0;
 err:
-	pci_free_consistent(qdev->pdev, tx_ring->wq_size,
-			    tx_ring->wq_base, tx_ring->wq_base_dma);
+	dma_free_coherent(&qdev->pdev->dev, tx_ring->wq_size,
+			  tx_ring->wq_base, tx_ring->wq_base_dma);
 	tx_ring->wq_base = NULL;
 pci_alloc_err:
 	netif_err(qdev, ifup, qdev->ndev, "tx_ring alloc failed.\n");
@@ -2748,17 +2733,17 @@ static void ql_free_lbq_buffers(struct ql_adapter *qdev, struct rx_ring *rx_ring
 			&lbq->queue[lbq->next_to_clean];
 
 		if (lbq_desc->p.pg_chunk.offset == last_offset)
-			pci_unmap_page(qdev->pdev, lbq_desc->dma_addr,
+			dma_unmap_page(&qdev->pdev->dev, lbq_desc->dma_addr,
 				       ql_lbq_block_size(qdev),
-				       PCI_DMA_FROMDEVICE);
+				       DMA_FROM_DEVICE);
 		put_page(lbq_desc->p.pg_chunk.page);
 
 		lbq->next_to_clean = QLGE_BQ_WRAP(lbq->next_to_clean + 1);
 	}
 
 	if (rx_ring->master_chunk.page) {
-		pci_unmap_page(qdev->pdev, rx_ring->chunk_dma_addr,
-			       ql_lbq_block_size(qdev), PCI_DMA_FROMDEVICE);
+		dma_unmap_page(&qdev->pdev->dev, rx_ring->chunk_dma_addr,
+			       ql_lbq_block_size(qdev), DMA_FROM_DEVICE);
 		put_page(rx_ring->master_chunk.page);
 		rx_ring->master_chunk.page = NULL;
 	}
@@ -2777,9 +2762,9 @@ static void ql_free_sbq_buffers(struct ql_adapter *qdev, struct rx_ring *rx_ring
 			return;
 		}
 		if (sbq_desc->p.skb) {
-			pci_unmap_single(qdev->pdev, sbq_desc->dma_addr,
+			dma_unmap_single(&qdev->pdev->dev, sbq_desc->dma_addr,
 					 SMALL_BUF_MAP_SIZE,
-					 PCI_DMA_FROMDEVICE);
+					 DMA_FROM_DEVICE);
 			dev_kfree_skb(sbq_desc->p.skb);
 			sbq_desc->p.skb = NULL;
 		}
@@ -2820,8 +2805,8 @@ static int qlge_init_bq(struct qlge_bq *bq)
 	__le64 *buf_ptr;
 	int i;
 
-	bq->base = pci_alloc_consistent(qdev->pdev, QLGE_BQ_SIZE,
-					&bq->base_dma);
+	bq->base = dma_alloc_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				      &bq->base_dma, GFP_ATOMIC);
 	if (!bq->base) {
 		netif_err(qdev, ifup, qdev->ndev,
 			  "ring %u %s allocation failed.\n", rx_ring->cq_id,
@@ -2850,8 +2835,8 @@ static void ql_free_rx_resources(struct ql_adapter *qdev,
 {
 	/* Free the small buffer queue. */
 	if (rx_ring->sbq.base) {
-		pci_free_consistent(qdev->pdev, QLGE_BQ_SIZE,
-				    rx_ring->sbq.base, rx_ring->sbq.base_dma);
+		dma_free_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				  rx_ring->sbq.base, rx_ring->sbq.base_dma);
 		rx_ring->sbq.base = NULL;
 	}
 
@@ -2861,8 +2846,8 @@ static void ql_free_rx_resources(struct ql_adapter *qdev,
 
 	/* Free the large buffer queue. */
 	if (rx_ring->lbq.base) {
-		pci_free_consistent(qdev->pdev, QLGE_BQ_SIZE,
-				    rx_ring->lbq.base, rx_ring->lbq.base_dma);
+		dma_free_coherent(&qdev->pdev->dev, QLGE_BQ_SIZE,
+				  rx_ring->lbq.base, rx_ring->lbq.base_dma);
 		rx_ring->lbq.base = NULL;
 	}
 
@@ -2872,9 +2857,9 @@ static void ql_free_rx_resources(struct ql_adapter *qdev,
 
 	/* Free the rx queue. */
 	if (rx_ring->cq_base) {
-		pci_free_consistent(qdev->pdev,
-				    rx_ring->cq_size,
-				    rx_ring->cq_base, rx_ring->cq_base_dma);
+		dma_free_coherent(&qdev->pdev->dev,
+				  rx_ring->cq_size,
+				  rx_ring->cq_base, rx_ring->cq_base_dma);
 		rx_ring->cq_base = NULL;
 	}
 }
@@ -2890,8 +2875,8 @@ static int ql_alloc_rx_resources(struct ql_adapter *qdev,
 	 * Allocate the completion queue for this rx_ring.
 	 */
 	rx_ring->cq_base =
-	    pci_alloc_consistent(qdev->pdev, rx_ring->cq_size,
-				 &rx_ring->cq_base_dma);
+	    dma_alloc_coherent(&qdev->pdev->dev, rx_ring->cq_size,
+			       &rx_ring->cq_base_dma, GFP_ATOMIC);
 
 	if (!rx_ring->cq_base) {
 		netif_err(qdev, ifup, qdev->ndev, "rx_ring alloc failed.\n");
@@ -3008,7 +2993,7 @@ static int ql_start_rx_ring(struct ql_adapter *qdev, struct rx_ring *rx_ring)
 	rx_ring->sbq.base_indirect_dma = shadow_reg_dma;
 
 	/* PCI doorbell mem area + 0x00 for consumer index register */
-	rx_ring->cnsmr_idx_db_reg = (u32 __iomem *) doorbell_area;
+	rx_ring->cnsmr_idx_db_reg = (u32 __iomem *)doorbell_area;
 	rx_ring->cnsmr_idx = 0;
 	rx_ring->curr_entry = rx_ring->cq_base;
 
@@ -3108,7 +3093,7 @@ static int ql_start_tx_ring(struct ql_adapter *qdev, struct tx_ring *tx_ring)
 	 * Assign doorbell registers for this tx_ring.
 	 */
 	/* TX PCI doorbell mem area for tx producer index */
-	tx_ring->prod_idx_db_reg = (u32 __iomem *) doorbell_area;
+	tx_ring->prod_idx_db_reg = (u32 __iomem *)doorbell_area;
 	tx_ring->prod_idx = 0;
 	/* TX PCI doorbell mem area + 0x04 */
 	tx_ring->valid_db_reg = doorbell_area + 0x04;
@@ -3131,7 +3116,7 @@ static int ql_start_tx_ring(struct ql_adapter *qdev, struct tx_ring *tx_ring)
 	ql_init_tx_ring(qdev, tx_ring);
 
 	err = ql_write_cfg(qdev, wqicb, sizeof(*wqicb), CFG_LRQ,
-			   (u16) tx_ring->wq_id);
+			   (u16)tx_ring->wq_id);
 	if (err) {
 		netif_err(qdev, ifup, qdev->ndev, "Failed to load tx_ring.\n");
 		return err;
@@ -3431,9 +3416,9 @@ static int ql_request_irq(struct ql_adapter *qdev)
 				     &qdev->rx_ring[0]);
 			status =
 			    request_irq(pdev->irq, qlge_isr,
-					test_bit(QL_MSI_ENABLED,
-						 &qdev->
-						 flags) ? 0 : IRQF_SHARED,
+					test_bit(QL_MSI_ENABLED, &qdev->flags)
+						? 0
+						: IRQF_SHARED,
 					intr_context->name, &qdev->rx_ring[0]);
 			if (status)
 				goto err_irq;
@@ -3463,7 +3448,7 @@ static int ql_start_rss(struct ql_adapter *qdev)
 	struct ricb *ricb = &qdev->ricb;
 	int status = 0;
 	int i;
-	u8 *hash_id = (u8 *) ricb->hash_cq_id;
+	u8 *hash_id = (u8 *)ricb->hash_cq_id;
 
 	memset((void *)ricb, 0, sizeof(*ricb));
 
@@ -4125,11 +4110,11 @@ static struct net_device_stats *qlge_get_stats(struct net_device
 	/* Get RX stats. */
 	pkts = mcast = dropped = errors = bytes = 0;
 	for (i = 0; i < qdev->rss_ring_count; i++, rx_ring++) {
-			pkts += rx_ring->rx_packets;
-			bytes += rx_ring->rx_bytes;
-			dropped += rx_ring->rx_dropped;
-			errors += rx_ring->rx_errors;
-			mcast += rx_ring->rx_multicast;
+		pkts += rx_ring->rx_packets;
+		bytes += rx_ring->rx_bytes;
+		dropped += rx_ring->rx_dropped;
+		errors += rx_ring->rx_errors;
+		mcast += rx_ring->rx_multicast;
 	}
 	ndev->stats.rx_packets = pkts;
 	ndev->stats.rx_bytes = bytes;
@@ -4140,9 +4125,9 @@ static struct net_device_stats *qlge_get_stats(struct net_device
 	/* Get TX stats. */
 	pkts = errors = bytes = 0;
 	for (i = 0; i < qdev->tx_ring_count; i++, tx_ring++) {
-			pkts += tx_ring->tx_packets;
-			bytes += tx_ring->tx_bytes;
-			errors += tx_ring->tx_errors;
+		pkts += tx_ring->tx_packets;
+		bytes += tx_ring->tx_bytes;
+		errors += tx_ring->tx_errors;
 	}
 	ndev->stats.tx_packets = pkts;
 	ndev->stats.tx_bytes = bytes;
@@ -4218,7 +4203,7 @@ static void qlge_set_multicast_list(struct net_device *ndev)
 			goto exit;
 		i = 0;
 		netdev_for_each_mc_addr(ha, ndev) {
-			if (ql_set_mac_addr_reg(qdev, (u8 *) ha->addr,
+			if (ql_set_mac_addr_reg(qdev, (u8 *)ha->addr,
 						MAC_ADDR_TYPE_MULTI_MAC, i)) {
 				netif_err(qdev, hw, qdev->ndev,
 					  "Failed to loadmulticast address.\n");
@@ -4255,7 +4240,7 @@ static int qlge_set_mac_address(struct net_device *ndev, void *p)
 	status = ql_sem_spinlock(qdev, SEM_MAC_ADDR_MASK);
 	if (status)
 		return status;
-	status = ql_set_mac_addr_reg(qdev, (u8 *) ndev->dev_addr,
+	status = ql_set_mac_addr_reg(qdev, (u8 *)ndev->dev_addr,
 				     MAC_ADDR_TYPE_CAM_MAC,
 				     qdev->func * MAX_CQ);
 	if (status)
@@ -4430,13 +4415,14 @@ static int ql_init_device(struct pci_dev *pdev, struct net_device *ndev,
 	}
 
 	pci_set_master(pdev);
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
+	if (!dma_set_mask(&pdev->dev, DMA_BIT_MASK(64))) {
 		set_bit(QL_DMA64, &qdev->flags);
-		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+		err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
 	} else {
-		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
 		if (!err)
-		       err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+			err = dma_set_coherent_mask(&pdev->dev,
+						    DMA_BIT_MASK(32));
 	}
 
 	if (err) {
@@ -4448,8 +4434,7 @@ static int ql_init_device(struct pci_dev *pdev, struct net_device *ndev,
 	pdev->needs_freset = 1;
 	pci_save_state(pdev);
 	qdev->reg_base =
-	    ioremap(pci_resource_start(pdev, 1),
-			    pci_resource_len(pdev, 1));
+		ioremap(pci_resource_start(pdev, 1), pci_resource_len(pdev, 1));
 	if (!qdev->reg_base) {
 		dev_err(&pdev->dev, "Register mapping failed.\n");
 		err = -ENOMEM;
@@ -4458,8 +4443,7 @@ static int ql_init_device(struct pci_dev *pdev, struct net_device *ndev,
 
 	qdev->doorbell_area_size = pci_resource_len(pdev, 3);
 	qdev->doorbell_area =
-	    ioremap(pci_resource_start(pdev, 3),
-			    pci_resource_len(pdev, 3));
+		ioremap(pci_resource_start(pdev, 3), pci_resource_len(pdev, 3));
 	if (!qdev->doorbell_area) {
 		dev_err(&pdev->dev, "Doorbell register mapping failed.\n");
 		err = -ENOMEM;
diff --git a/drivers/staging/rtl8188eu/core/rtw_ap.c b/drivers/staging/rtl8188eu/core/rtw_ap.c
index 93283c7deec4..817793b9aff2 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ap.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ap.c
@@ -264,8 +264,10 @@ void expire_timeout_chk(struct adapter *padapter)
 			list_del_init(&psta->asoc_list);
 			pstapriv->asoc_list_cnt--;
 
-			DBG_88E("asoc expire %pM, state = 0x%x\n", (psta->hwaddr), psta->state);
-			updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+			DBG_88E("asoc expire %pM, state = 0x%x\n",
+				(psta->hwaddr), psta->state);
+			updated = ap_free_sta(padapter, psta, true,
+					      WLAN_REASON_DEAUTH_LEAVING);
 		} else {
 			/* TODO: Aging mechanism to digest frames in sleep_q to avoid running out of xmitframe */
 			if (psta->sleepq_len > (NR_XMITFRAME / pstapriv->asoc_list_cnt) &&
@@ -294,16 +296,21 @@ void expire_timeout_chk(struct adapter *padapter)
 		for (i = 0; i < chk_alive_num; i++) {
 			int ret = _FAIL;
 
-			psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
+			psta = rtw_get_stainfo_by_offset(pstapriv,
+							 chk_alive_list[i]);
 
-			if (psta->state & WIFI_SLEEP_STATE)
-				ret = issue_nulldata(padapter, psta->hwaddr, 0, 1, 50);
-			else
-				ret = issue_nulldata(padapter, psta->hwaddr, 0, 3, 50);
+			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_88E("asoc check, sta(%pM) is alive\n", (psta->hwaddr));
+				DBG_88E("asoc check, sta(%pM) is alive\n",
+					(psta->hwaddr));
 				psta->expire_to = pstapriv->expire_to;
 				psta->keep_alive_trycnt = 0;
 				continue;
@@ -315,11 +322,13 @@ void expire_timeout_chk(struct adapter *padapter)
 
 			psta->keep_alive_trycnt = 0;
 
-			DBG_88E("asoc expire %pM, state = 0x%x\n", (psta->hwaddr), psta->state);
+			DBG_88E("asoc expire %pM, state = 0x%x\n",
+				psta->hwaddr, psta->state);
 			spin_lock_bh(&pstapriv->asoc_list_lock);
 			list_del_init(&psta->asoc_list);
 			pstapriv->asoc_list_cnt--;
-			updated = ap_free_sta(padapter, psta, true, WLAN_REASON_DEAUTH_LEAVING);
+			updated = ap_free_sta(padapter, psta, true,
+					      WLAN_REASON_DEAUTH_LEAVING);
 			spin_unlock_bh(&pstapriv->asoc_list_lock);
 		}
 
@@ -431,7 +440,8 @@ static void update_bmc_sta(struct adapter *padapter)
 		supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->SupportedRates);
 		network_type = rtw_check_network_type((u8 *)&pcur_network->SupportedRates);
 
-		memcpy(psta->bssrateset, &pcur_network->SupportedRates, supportRateNum);
+		memcpy(psta->bssrateset, &pcur_network->SupportedRates,
+		       supportRateNum);
 		psta->bssratelen = supportRateNum;
 
 		/* b/g mode ra_bitmap */
@@ -445,7 +455,8 @@ static void update_bmc_sta(struct adapter *padapter)
 		tx_ra_bitmap = 0xf;
 
 		raid = networktype_to_raid(network_type);
-		init_rate = get_highest_rate_idx(tx_ra_bitmap & 0x0fffffff) & 0x3f;
+		init_rate = get_highest_rate_idx(tx_ra_bitmap & 0x0fffffff) &
+			    0x3f;
 
 		/* ap mode */
 		rtw_hal_set_odm_var(padapter, HAL_ODM_STA_INFO, psta, true);
@@ -456,7 +467,8 @@ static void update_bmc_sta(struct adapter *padapter)
 			arg = psta->mac_id & 0x1f;
 			arg |= BIT(7);
 			tx_ra_bitmap |= ((raid << 28) & 0xf0000000);
-			DBG_88E("%s, mask = 0x%x, arg = 0x%x\n", __func__, tx_ra_bitmap, arg);
+			DBG_88E("%s, mask = 0x%x, arg = 0x%x\n", __func__,
+				tx_ra_bitmap, arg);
 
 			/* bitmap[0:27] = tx_rate_bitmap */
 			/* bitmap[28:31]= Rate Adaptive id */
@@ -647,7 +659,8 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 	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_BEACON_INTERVAL,
+			  (u8 *)(&bcn_interval));
 
 	UpdateBrateTbl(padapter, pnetwork->SupportedRates);
 	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->SupportedRates);
@@ -657,7 +670,10 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 		Switch_DM_Func(padapter, DYNAMIC_ALL_FUNC_ENABLE, true);
 	}
 	/* set channel, bwmode */
-	p = rtw_get_ie((pnetwork->ies + sizeof(struct ndis_802_11_fixed_ie)), _HT_ADD_INFO_IE_, &ie_len, (pnetwork->ie_length - sizeof(struct ndis_802_11_fixed_ie)));
+	p = rtw_get_ie(pnetwork->ies + sizeof(struct ndis_802_11_fixed_ie),
+		       _HT_ADD_INFO_IE_, &ie_len,
+		       pnetwork->ie_length -
+		       sizeof(struct ndis_802_11_fixed_ie));
 	if (p && ie_len) {
 		pht_info = (struct HT_info_element *)(p + 2);
 
@@ -682,7 +698,8 @@ static void start_bss_network(struct adapter *padapter, u8 *pbuf)
 	 */
 	set_channel_bwmode(padapter, cur_channel, cur_ch_offset, cur_bwmode);
 
-	DBG_88E("CH =%d, BW =%d, offset =%d\n", cur_channel, cur_bwmode, cur_ch_offset);
+	DBG_88E("CH =%d, BW =%d, offset =%d\n", cur_channel, cur_bwmode,
+		cur_ch_offset);
 
 	/*  */
 	pmlmeext->cur_channel = cur_channel;
@@ -771,17 +788,19 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	cap = get_unaligned_le16(ie);
 
 	/* SSID */
-	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len, (pbss_network->ie_length - _BEACON_IE_OFFSET_));
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SSID_IE_, &ie_len,
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p && ie_len > 0) {
 		memset(&pbss_network->ssid, 0, sizeof(struct ndis_802_11_ssid));
-		memcpy(pbss_network->ssid.ssid, (p + 2), ie_len);
+		memcpy(pbss_network->ssid.ssid, p + 2, ie_len);
 		pbss_network->ssid.ssid_length = ie_len;
 	}
 
 	/* channel */
 	channel = 0;
 	pbss_network->Configuration.Length = 0;
-	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len, (pbss_network->ie_length - _BEACON_IE_OFFSET_));
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _DSSET_IE_, &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		channel = *(p + 2);
 
@@ -789,14 +808,16 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 
 	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->ie_length - _BEACON_IE_OFFSET_));
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _SUPPORTEDRATES_IE_, &ie_len,
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p) {
 		memcpy(supportRate, p + 2, ie_len);
 		supportRateNum = ie_len;
 	}
 
 	/* get ext_supported rates */
-	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_, &ie_len, pbss_network->ie_length - _BEACON_IE_OFFSET_);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _EXT_SUPPORTEDRATES_IE_,
+		       &ie_len, pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p) {
 		memcpy(supportRate + supportRateNum, p + 2, ie_len);
 		supportRateNum += ie_len;
@@ -807,7 +828,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	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->ie_length - _BEACON_IE_OFFSET_));
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _ERPINFO_IE_, &ie_len,
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p && ie_len > 0)
 		ERP_IE_handler(padapter, (struct ndis_802_11_var_ie *)p);
 
@@ -824,7 +846,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	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->ie_length - _BEACON_IE_OFFSET_));
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _RSN_IE_2_, &ie_len,
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p && ie_len > 0) {
 		if (rtw_parse_wpa2_ie(p, ie_len + 2, &group_cipher, &pairwise_cipher, NULL) == _SUCCESS) {
 			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
@@ -844,7 +867,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	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->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+			       pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2));
 		if ((p) && (!memcmp(p + 2, OUI1, 4))) {
 			if (rtw_parse_wpa_ie(p, ie_len + 2, &group_cipher,
 					     &pairwise_cipher, NULL) == _SUCCESS) {
@@ -869,7 +892,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	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->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2)));
+				       pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2));
 			if ((p) && !memcmp(p + 2, WMM_PARA_IE, 6)) {
 				pmlmepriv->qospriv.qos_option = 1;
 
@@ -892,7 +915,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	}
 	/* parsing HT_CAP_IE */
 	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_CAPABILITY_IE_, &ie_len,
-		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p && ie_len > 0) {
 		struct ieee80211_ht_cap *pht_cap = (struct ieee80211_ht_cap *)(p + 2);
 
@@ -916,7 +939,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 
 	/* parsing HT_INFO_IE */
 	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_, _HT_ADD_INFO_IE_, &ie_len,
-		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p && ie_len > 0)
 		pHT_info_ie = p;
 	switch (network_type) {
@@ -1226,17 +1249,17 @@ void update_beacon(struct adapter *padapter, u8 ie_id, u8 *oui, u8 tx)
 }
 
 /*
-op_mode
-Set to 0 (HT pure) under the following conditions
-	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
-	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS
-Set to 1 (HT non-member protection) if there may be non-HT STAs
-	in both the primary and the secondary channel
-Set to 2 if only HT STAs are associated in BSS,
-	however and at least one 20 MHz HT STA is associated
-Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
-	(currently non-GF HT station is considered as non-HT STA also)
-*/
+ * op_mode
+ * Set to 0 (HT pure) under the following conditions
+ *	- all STAs in the BSS are 20/40 MHz HT in 20/40 MHz BSS or
+ *	- all STAs in the BSS are 20 MHz HT in 20 MHz BSS
+ * Set to 1 (HT non-member protection) if there may be non-HT STAs
+ *	  in both the primary and the secondary channel
+ * Set to 2 if only HT STAs are associated in BSS,
+ *	  however and at least one 20 MHz HT STA is associated
+ * Set to 3 (HT mixed mode) when one or more non-HT STAs are associated
+ *	  (currently non-GF HT station is considered as non-HT STA also)
+ */
 static int rtw_ht_operation_update(struct adapter *padapter)
 {
 	u16 cur_op_mode, new_op_mode;
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index c525682d0edf..9bb3ec0cd62f 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -370,28 +370,27 @@ static u16 Efuse_GetCurrentSize(struct adapter *pAdapter)
 
 	while (efuse_OneByteRead(pAdapter, efuse_addr, &efuse_data) &&
 	       AVAILABLE_EFUSE_ADDR(efuse_addr)) {
-		if (efuse_data != 0xFF) {
-			if ((efuse_data & 0x1F) == 0x0F) {		/* extended header */
-				hoffset = efuse_data;
+		if (efuse_data == 0xFF)
+			break;
+		if ((efuse_data & 0x1F) == 0x0F) { /* extended header */
+			hoffset = efuse_data;
+			efuse_addr++;
+			efuse_OneByteRead(pAdapter, efuse_addr, &efuse_data);
+			if ((efuse_data & 0x0F) == 0x0F) {
 				efuse_addr++;
-				efuse_OneByteRead(pAdapter, efuse_addr, &efuse_data);
-				if ((efuse_data & 0x0F) == 0x0F) {
-					efuse_addr++;
-					continue;
-				} else {
-					hoffset = ((hoffset & 0xE0) >> 5) | ((efuse_data & 0xF0) >> 1);
-					hworden = efuse_data & 0x0F;
-				}
+				continue;
 			} else {
-				hoffset = (efuse_data >> 4) & 0x0F;
-				hworden =  efuse_data & 0x0F;
+				hoffset = ((hoffset & 0xE0) >> 5) |
+					  ((efuse_data & 0xF0) >> 1);
+				hworden = efuse_data & 0x0F;
 			}
-			word_cnts = Efuse_CalculateWordCnts(hworden);
-			/* read next header */
-			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
 		} else {
-			break;
+			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;
 	}
 
 	rtw_hal_set_hwreg(pAdapter, HW_VAR_EFUSE_BYTES, (u8 *)&efuse_addr);
diff --git a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
index e186982d5908..caf600eba03b 100644
--- a/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8188eu/core/rtw_ieee80211.c
@@ -253,11 +253,11 @@ int rtw_generate_ie(struct registry_priv *pregistrypriv)
 	}
 
 	/* DS parameter set */
-	ie = rtw_set_ie(ie, _DSSET_IE_, 1, (u8 *)&(pdev_network->Configuration.DSConfig), &sz);
+	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);
+	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);
diff --git a/drivers/staging/rtl8188eu/core/rtw_led.c b/drivers/staging/rtl8188eu/core/rtw_led.c
index d1406cc99768..32dccae186ca 100644
--- a/drivers/staging/rtl8188eu/core/rtw_led.c
+++ b/drivers/staging/rtl8188eu/core/rtw_led.c
@@ -90,7 +90,6 @@ static void SwLedBlink1(struct LED_871x *pLed)
 {
 	struct adapter *padapter = pLed->padapter;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	u8 bStopBlinking = false;
 
 	/*  Change LED according to BlinkingLedState specified. */
 	if (pLed->BlinkingLedState == RTW_LED_ON) {
@@ -128,9 +127,7 @@ static void SwLedBlink1(struct LED_871x *pLed)
 		break;
 	case LED_BLINK_SCAN:
 		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
+		if (pLed->BlinkTimes == 0) {
 			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
@@ -164,9 +161,7 @@ static void SwLedBlink1(struct LED_871x *pLed)
 		break;
 	case LED_BLINK_TXRX:
 		pLed->BlinkTimes--;
-		if (pLed->BlinkTimes == 0)
-			bStopBlinking = true;
-		if (bStopBlinking) {
+		if (pLed->BlinkTimes == 0) {
 			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				pLed->bLedLinkBlinkInProgress = true;
 				pLed->CurrLedState = LED_BLINK_NORMAL;
@@ -188,7 +183,6 @@ static void SwLedBlink1(struct LED_871x *pLed)
 					  msecs_to_jiffies(LED_BLINK_NO_LINK_INTERVAL_ALPHA));
 				RT_TRACE(_module_rtl8712_led_c_, _drv_info_, ("CurrLedState %d\n", pLed->CurrLedState));
 			}
-			pLed->BlinkTimes = 0;
 			pLed->bLedBlinkInProgress = false;
 		} else {
 			if (pLed->bLedOn)
@@ -208,12 +202,7 @@ static void SwLedBlink1(struct LED_871x *pLed)
 			  msecs_to_jiffies(LED_BLINK_SCAN_INTERVAL_ALPHA));
 		break;
 	case LED_BLINK_WPS_STOP:	/* WPS success */
-		if (pLed->BlinkingLedState == RTW_LED_ON)
-			bStopBlinking = false;
-		else
-			bStopBlinking = true;
-
-		if (bStopBlinking) {
+		if (pLed->BlinkingLedState != RTW_LED_ON) {
 			pLed->bLedLinkBlinkInProgress = true;
 			pLed->CurrLedState = LED_BLINK_NORMAL;
 			if (pLed->bLedOn)
diff --git a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
index 04897cd48370..8d035f67ef61 100644
--- a/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8188eu/core/rtw_mlme_ext.c
@@ -1781,7 +1781,7 @@ static void issue_action_BSSCoexistPacket(struct adapter *padapter)
 
 			p = rtw_get_ie(pbss_network->ies + _FIXED_IE_LENGTH_, _HT_CAPABILITY_IE_, &len, pbss_network->ie_length - _FIXED_IE_LENGTH_);
 			if (!p || len == 0) { /* non-HT */
-				if ((pbss_network->Configuration.DSConfig <= 0) || (pbss_network->Configuration.DSConfig > 14))
+				if (pbss_network->Configuration.DSConfig <= 0)
 					continue;
 
 				ICS[0][pbss_network->Configuration.DSConfig] = 1;
@@ -1932,11 +1932,11 @@ static void site_survey(struct adapter *padapter)
 				if (pmlmeext->sitesurvey_res.ssid[i].ssid_length) {
 					/* todo: to issue two probe req??? */
 					issue_probereq(padapter,
-					&(pmlmeext->sitesurvey_res.ssid[i]),
+					&pmlmeext->sitesurvey_res.ssid[i],
 								NULL, false);
 					/* msleep(SURVEY_TO>>1); */
 					issue_probereq(padapter,
-					&(pmlmeext->sitesurvey_res.ssid[i]),
+					&pmlmeext->sitesurvey_res.ssid[i],
 								NULL, false);
 				}
 			}
diff --git a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
index c4f58507dbfd..c000382c96d9 100644
--- a/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8188eu/core/rtw_pwrctrl.c
@@ -173,7 +173,7 @@ int ips_leave(struct adapter *padapter)
 
 		DBG_88E_LEVEL(_drv_info_, "nolinked power save leave\n");
 
-		if ((_WEP40_ == psecuritypriv->dot11PrivacyAlgrthm) || (_WEP104_ == psecuritypriv->dot11PrivacyAlgrthm)) {
+		if ((psecuritypriv->dot11PrivacyAlgrthm == _WEP40_) || (psecuritypriv->dot11PrivacyAlgrthm == _WEP104_)) {
 			DBG_88E("==>%s, channel(%d), processing(%x)\n", __func__, padapter->mlmeextpriv.cur_channel, pwrpriv->bips_processing);
 			set_channel_bwmode(padapter, padapter->mlmeextpriv.cur_channel, HAL_PRIME_CHNL_OFFSET_DONT_CARE, HT_CHANNEL_WIDTH_20);
 			for (keyid = 0; keyid < 4; keyid++) {
diff --git a/drivers/staging/rtl8188eu/core/rtw_recv.c b/drivers/staging/rtl8188eu/core/rtw_recv.c
index d4278361e002..a036ef104198 100644
--- a/drivers/staging/rtl8188eu/core/rtw_recv.c
+++ b/drivers/staging/rtl8188eu/core/rtw_recv.c
@@ -1525,21 +1525,14 @@ static int amsdu_to_msdu(struct adapter *padapter, struct recv_frame *prframe)
 
 		/* Allocate new skb for releasing to upper layer */
 		sub_skb = dev_alloc_skb(nSubframe_Length + 12);
-		if (sub_skb) {
-			skb_reserve(sub_skb, 12);
-			skb_put_data(sub_skb, pdata, nSubframe_Length);
-		} else {
-			sub_skb = skb_clone(prframe->pkt, GFP_ATOMIC);
-			if (sub_skb) {
-				sub_skb->data = pdata;
-				sub_skb->len = nSubframe_Length;
-				skb_set_tail_pointer(sub_skb, nSubframe_Length);
-			} else {
-				DBG_88E("skb_clone() Fail!!! , nr_subframes=%d\n", nr_subframes);
-				break;
-			}
+		if (!sub_skb) {
+			DBG_88E("dev_alloc_skb() Fail!!! , nr_subframes=%d\n", nr_subframes);
+			break;
 		}
 
+		skb_reserve(sub_skb, 12);
+		skb_put_data(sub_skb, pdata, nSubframe_Length);
+
 		subframes[nr_subframes++] = sub_skb;
 
 		if (nr_subframes >= MAX_SUBFRAME_COUNT) {
diff --git a/drivers/staging/rtl8188eu/hal/fw.c b/drivers/staging/rtl8188eu/hal/fw.c
index 486ee4bd4744..3d1d29e9f8e0 100644
--- a/drivers/staging/rtl8188eu/hal/fw.c
+++ b/drivers/staging/rtl8188eu/hal/fw.c
@@ -111,7 +111,7 @@ static int _rtl88e_fw_free_to_go(struct adapter *adapt)
 
 	do {
 		value32 = usb_read32(adapt, REG_MCUFWDL);
-		if (value32 & FWDL_ChkSum_rpt)
+		if (value32 & FWDL_CHKSUM_RPT)
 			break;
 	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
 
@@ -146,7 +146,7 @@ int rtl88eu_download_fw(struct adapter *adapt)
 	struct dvobj_priv *dvobj = adapter_to_dvobj(adapt);
 	struct device *device = dvobj_to_dev(dvobj);
 	const struct firmware *fw;
-	const char fw_name[] = "rtlwifi/rtl8188eufw.bin";
+	static const char fw_name[] = "rtlwifi/rtl8188eufw.bin";
 	struct rtl92c_firmware_header *pfwheader = NULL;
 	u8 *download_data, *fw_data;
 	size_t download_size;
@@ -192,7 +192,8 @@ int rtl88eu_download_fw(struct adapter *adapt)
 		rtl88e_firmware_selfreset(adapt);
 	}
 	_rtl88e_enable_fw_download(adapt, true);
-	usb_write8(adapt, REG_MCUFWDL, usb_read8(adapt, REG_MCUFWDL) | FWDL_ChkSum_rpt);
+	usb_write8(adapt, REG_MCUFWDL,
+		   usb_read8(adapt, REG_MCUFWDL) | FWDL_CHKSUM_RPT);
 	_rtl88e_write_fw(adapt, download_data, download_size);
 	_rtl88e_enable_fw_download(adapt, false);
 
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index 698377ea60ee..28974808839d 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -5,8 +5,6 @@
  *
  ******************************************************************************/
 
-/*  include files */
-
 #include "odm_precomp.h"
 #include "phy.h"
 
@@ -193,7 +191,7 @@ void ODM_DMWatchdog(struct odm_dm_struct *pDM_Odm)
 	odm_DIG(pDM_Odm);
 	odm_CCKPacketDetectionThresh(pDM_Odm);
 
-	if (*(pDM_Odm->pbPowerSaving))
+	if (*pDM_Odm->pbPowerSaving)
 		return;
 
 	odm_RefreshRateAdaptiveMask(pDM_Odm);
@@ -229,13 +227,13 @@ void odm_CommonInfoSelfUpdate(struct odm_dm_struct *pDM_Odm)
 	u8 i;
 	struct sta_info *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;
+	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);
+		pDM_Odm->ControlChannel = *pDM_Odm->pChannel;
 	}
 
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
@@ -270,16 +268,16 @@ void odm_CmnInfoInit_Debug(struct odm_dm_struct *pDM_Odm)
 void odm_CmnInfoHook_Debug(struct odm_dm_struct *pDM_Odm)
 {
 	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("odm_CmnInfoHook_Debug==>\n"));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pNumTxBytesUnicast=%llu\n", *(pDM_Odm->pNumTxBytesUnicast)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pNumRxBytesUnicast=%llu\n", *(pDM_Odm->pNumRxBytesUnicast)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pWirelessMode=0x%x\n", *(pDM_Odm->pWirelessMode)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pSecChOffset=%d\n", *(pDM_Odm->pSecChOffset)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pSecurity=%d\n", *(pDM_Odm->pSecurity)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pBandWidth=%d\n", *(pDM_Odm->pBandWidth)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pChannel=%d\n", *(pDM_Odm->pChannel)));
-
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pbScanInProcess=%d\n", *(pDM_Odm->pbScanInProcess)));
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pbPowerSaving=%d\n", *(pDM_Odm->pbPowerSaving)));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pNumTxBytesUnicast=%llu\n", *pDM_Odm->pNumTxBytesUnicast));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pNumRxBytesUnicast=%llu\n", *pDM_Odm->pNumRxBytesUnicast));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pWirelessMode=0x%x\n", *pDM_Odm->pWirelessMode));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pSecChOffset=%d\n", *pDM_Odm->pSecChOffset));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pSecurity=%d\n", *pDM_Odm->pSecurity));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pBandWidth=%d\n", *pDM_Odm->pBandWidth));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pChannel=%d\n", *pDM_Odm->pChannel));
+
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pbScanInProcess=%d\n", *pDM_Odm->pbScanInProcess));
+	ODM_RT_TRACE(pDM_Odm, ODM_COMP_COMMON, ODM_DBG_LOUD, ("pbPowerSaving=%d\n", *pDM_Odm->pbPowerSaving));
 }
 
 void odm_CmnInfoUpdate_Debug(struct odm_dm_struct *pDM_Odm)
@@ -348,7 +346,7 @@ void odm_DIG(struct odm_dm_struct *pDM_Odm)
 		return;
 	}
 
-	if (*(pDM_Odm->pbScanInProcess)) {
+	if (*pDM_Odm->pbScanInProcess) {
 		ODM_RT_TRACE(pDM_Odm, ODM_COMP_DIG, ODM_DBG_LOUD, ("odm_DIG() Return: In Scan Progress\n"));
 		return;
 	}
@@ -508,7 +506,7 @@ void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
 {
 	struct adapter *adapter = pDM_Odm->Adapter;
 	u32 ret_value;
-	struct false_alarm_stats *FalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
 
 	if (!(pDM_Odm->SupportAbility & ODM_BB_FA_CNT))
 		return;
@@ -581,7 +579,7 @@ void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
 void odm_CCKPacketDetectionThresh(struct odm_dm_struct *pDM_Odm)
 {
 	u8 CurCCK_CCAThres;
-	struct false_alarm_stats *FalseAlmCnt = &(pDM_Odm->FalseAlmCnt);
+	struct false_alarm_stats *FalseAlmCnt = &pDM_Odm->FalseAlmCnt;
 
 	if (!(pDM_Odm->SupportAbility & (ODM_BB_CCK_PD | ODM_BB_FA_CNT)))
 		return;
@@ -739,7 +737,7 @@ u32 ODM_Get_Rate_Bitmap(struct odm_dm_struct *pDM_Odm, u32 macid, u32 ra_mask, u
 		} else if (rssi_level == DM_RATR_STA_MIDDLE) {
 			rate_bitmap = 0x000ff000;
 		} else {
-			if (*(pDM_Odm->pBandWidth) == ODM_BW40M)
+			if (*pDM_Odm->pBandWidth == ODM_BW40M)
 				rate_bitmap = 0x000ff015;
 			else
 				rate_bitmap = 0x000ff005;
@@ -945,7 +943,7 @@ void odm_TXPowerTrackingInit(struct odm_dm_struct *pDM_Odm)
 {
 	pDM_Odm->RFCalibrateInfo.bTXPowerTracking = true;
 	pDM_Odm->RFCalibrateInfo.TXPowercount = 0;
-	if (*(pDM_Odm->mp_mode) != 1)
+	if (*pDM_Odm->mp_mode != 1)
 		pDM_Odm->RFCalibrateInfo.TxPowerTrackControl = true;
 	MSG_88E("pDM_Odm TxPowerTrackControl = %d\n", pDM_Odm->RFCalibrateInfo.TxPowerTrackControl);
 
@@ -1035,11 +1033,11 @@ void odm_EdcaTurboCheckCE(struct odm_dm_struct *pDM_Odm)
 	u64	cur_tx_bytes = 0;
 	u64	cur_rx_bytes = 0;
 	u8	bbtchange = false;
-	struct xmit_priv		*pxmitpriv = &(Adapter->xmitpriv);
-	struct recv_priv		*precvpriv = &(Adapter->recvpriv);
+	struct xmit_priv		*pxmitpriv = &Adapter->xmitpriv;
+	struct recv_priv		*precvpriv = &Adapter->recvpriv;
 	struct registry_priv	*pregpriv = &Adapter->registrypriv;
-	struct mlme_ext_priv	*pmlmeext = &(Adapter->mlmeextpriv);
-	struct mlme_ext_info	*pmlmeinfo = &(pmlmeext->mlmext_info);
+	struct mlme_ext_priv	*pmlmeext = &Adapter->mlmeextpriv;
+	struct mlme_ext_info	*pmlmeinfo = &pmlmeext->mlmext_info;
 
 	if (pregpriv->wifi_spec == 1) /*  (pmlmeinfo->HT_enable == 0)) */
 		goto dm_CheckEdcaTurbo_EXIT;
diff --git a/drivers/staging/rtl8188eu/hal/odm_hwconfig.c b/drivers/staging/rtl8188eu/hal/odm_hwconfig.c
index a6f2731b076d..65a346ae3cb0 100644
--- a/drivers/staging/rtl8188eu/hal/odm_hwconfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_hwconfig.c
@@ -5,8 +5,6 @@
  *
  ******************************************************************************/
 
-/*  include files */
-
 #include "odm_precomp.h"
 
 #define READ_AND_CONFIG     READ_AND_CONFIG_MP
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
index b9025815b682..920688fc9e9f 100644
--- a/drivers/staging/rtl8188eu/hal/phy.c
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -345,8 +345,8 @@ static void dm_txpwr_track_setpwr(struct odm_dm_struct *dm_odm)
 {
 	if (dm_odm->BbSwingFlagOfdm || dm_odm->BbSwingFlagCck) {
 		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
-			     ("dm_txpwr_track_setpwr CH=%d\n", *(dm_odm->pChannel)));
-		phy_set_tx_power_level(dm_odm->Adapter, *(dm_odm->pChannel));
+			     ("dm_txpwr_track_setpwr CH=%d\n", *dm_odm->pChannel));
+		phy_set_tx_power_level(dm_odm->Adapter, *dm_odm->pChannel);
 		dm_odm->BbSwingFlagOfdm = false;
 		dm_odm->BbSwingFlagCck = false;
 	}
@@ -786,7 +786,7 @@ static void pathb_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8],
 	}
 }
 
-static void save_adda_registers(struct adapter *adapt, u32 *addareg,
+static void save_adda_registers(struct adapter *adapt, const u32 *addareg,
 				u32 *backup, u32 register_num)
 {
 	u32 i;
@@ -795,7 +795,7 @@ static void save_adda_registers(struct adapter *adapt, u32 *addareg,
 		backup[i] = phy_query_bb_reg(adapt, addareg[i], bMaskDWord);
 }
 
-static void save_mac_registers(struct adapter *adapt, u32 *mac_reg,
+static void save_mac_registers(struct adapter *adapt, const u32 *mac_reg,
 			       u32 *backup)
 {
 	u32 i;
@@ -806,7 +806,7 @@ static void save_mac_registers(struct adapter *adapt, u32 *mac_reg,
 	backup[i] = usb_read32(adapt, mac_reg[i]);
 }
 
-static void reload_adda_reg(struct adapter *adapt, u32 *adda_reg,
+static void reload_adda_reg(struct adapter *adapt, const u32 *adda_reg,
 			    u32 *backup, u32 regiester_num)
 {
 	u32 i;
@@ -815,8 +815,8 @@ static void reload_adda_reg(struct adapter *adapt, u32 *adda_reg,
 		phy_set_bb_reg(adapt, adda_reg[i], bMaskDWord, backup[i]);
 }
 
-static void reload_mac_registers(struct adapter *adapt,
-				 u32 *mac_reg, u32 *backup)
+static void reload_mac_registers(struct adapter *adapt, const u32 *mac_reg,
+				 u32 *backup)
 {
 	u32 i;
 
@@ -826,7 +826,7 @@ static void reload_mac_registers(struct adapter *adapt,
 	usb_write32(adapt, mac_reg[i], backup[i]);
 }
 
-static void path_adda_on(struct adapter *adapt, u32 *adda_reg,
+static void path_adda_on(struct adapter *adapt, const u32 *adda_reg,
 			 bool is_path_a_on, bool is2t)
 {
 	u32 path_on;
@@ -844,7 +844,8 @@ static void path_adda_on(struct adapter *adapt, u32 *adda_reg,
 		phy_set_bb_reg(adapt, adda_reg[i], bMaskDWord, path_on);
 }
 
-static void mac_setting_calibration(struct adapter *adapt, u32 *mac_reg, u32 *backup)
+static void mac_setting_calibration(struct adapter *adapt, const u32 *mac_reg,
+				    u32 *backup)
 {
 	u32 i = 0;
 
@@ -952,30 +953,31 @@ static void phy_iq_calibrate(struct adapter *adapt, s32 result[][8],
 	struct odm_dm_struct *dm_odm = &adapt->HalData->odmpriv;
 	u32 i;
 	u8 path_a_ok, path_b_ok;
-	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};
-
+	static const 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
+	};
+	static const u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+		REG_TXPAUSE, REG_BCN_CTRL,
+		REG_BCN_CTRL_1, REG_GPIO_MUXCFG
+	};
 	/* since 92C & 92D have the different define in IQK_BB_REG */
-	u32 iqk_bb_reg_92c[IQK_BB_REG_NUM] = {
-					      rOFDM0_TRxPathEnable, rOFDM0_TRMuxPar,
-					      rFPGA0_XCD_RFInterfaceSW, rConfig_AntA, rConfig_AntB,
-					      rFPGA0_XAB_RFInterfaceSW, rFPGA0_XA_RFInterfaceOE,
-					      rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD};
+	static const u32 iqk_bb_reg_92c[IQK_BB_REG_NUM] = {
+		rOFDM0_TRxPathEnable, rOFDM0_TRMuxPar,
+		rFPGA0_XCD_RFInterfaceSW, rConfig_AntA, rConfig_AntB,
+		rFPGA0_XAB_RFInterfaceSW, rFPGA0_XA_RFInterfaceOE,
+		rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD
+	};
 
 	u32 retry_count = 9;
 
-	if (*(dm_odm->mp_mode) == 1)
+	if (*dm_odm->mp_mode == 1)
 		retry_count = 9;
 	else
 		retry_count = 2;
@@ -1320,7 +1322,7 @@ void rtl88eu_phy_lc_calibrate(struct adapter *adapt)
 	if (singletone || carrier_sup)
 		return;
 
-	while (*(dm_odm->pbScanInProcess) && timecount < timeout) {
+	while (*dm_odm->pbScanInProcess && timecount < timeout) {
 		mdelay(50);
 		timecount += 50;
 	}
diff --git a/drivers/staging/rtl8188eu/hal/rf.c b/drivers/staging/rtl8188eu/hal/rf.c
index 00a9f692bb06..6702f263c770 100644
--- a/drivers/staging/rtl8188eu/hal/rf.c
+++ b/drivers/staging/rtl8188eu/hal/rf.c
@@ -79,7 +79,7 @@ void rtl88eu_phy_rf6052_set_cck_txpower(struct adapter *adapt, u8 *powerlevel)
 		}
 	}
 	for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-		ptr = (u8 *)(&(tx_agc[idx1]));
+		ptr = (u8 *)(&tx_agc[idx1]);
 		for (idx2 = 0; idx2 < 4; idx2++) {
 			if (*ptr > RF6052_MAX_TX_PWR)
 				*ptr = RF6052_MAX_TX_PWR;
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
index 371e746915dd..176716d3e903 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_cmd.c
@@ -256,7 +256,7 @@ static void ConstructBeacon(struct adapter *adapt, u8 *pframe, u32 *pLength)
 	pframe = rtw_set_ie(pframe, _SUPPORTEDRATES_IE_, min_t(u32, rate_len, 8), cur_network->SupportedRates, &pktlen);
 
 	/*  DS parameter set */
-	pframe = rtw_set_ie(pframe, _DSSET_IE_, 1, (unsigned char *)&(cur_network->Configuration.DSConfig), &pktlen);
+	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;
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
index 241f55b92808..1af919ff6d93 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_dm.c
@@ -29,9 +29,6 @@ static void dm_InitGPIOSetting(struct adapter *Adapter)
 	usb_write8(Adapter, REG_GPIO_MUXCFG, tmp1byte);
 }
 
-/*  */
-/*  functions */
-/*  */
 static void Init_ODM_ComInfo_88E(struct adapter *Adapter)
 {
 	struct hal_data_8188e *hal_data = Adapter->HalData;
diff --git a/drivers/staging/rtl8188eu/include/osdep_service.h b/drivers/staging/rtl8188eu/include/osdep_service.h
index c0114ad79788..0d3e4a6e7e85 100644
--- a/drivers/staging/rtl8188eu/include/osdep_service.h
+++ b/drivers/staging/rtl8188eu/include/osdep_service.h
@@ -50,7 +50,7 @@ struct	__queue	{
 
 static inline struct list_head *get_list_head(struct __queue *queue)
 {
-	return &(queue->queue);
+	return &queue->queue;
 }
 
 static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_spec.h b/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
index dd943c831d91..be30c9434a29 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_spec.h
@@ -817,7 +817,7 @@ So the following defines for 92C is not entire!!!!!!
 /* 2 MCUFWDL */
 #define MCUFWDL_EN			BIT(0)
 #define MCUFWDL_RDY			BIT(1)
-#define FWDL_ChkSum_rpt			BIT(2)
+#define FWDL_CHKSUM_RPT			BIT(2)
 #define MACINI_RDY			BIT(3)
 #define BBINI_RDY			BIT(4)
 #define RFINI_RDY			BIT(5)
diff --git a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
index 9a89791720e0..d5968ef9f43d 100644
--- a/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8188eu/os_dep/ioctl_linux.c
@@ -93,7 +93,7 @@ static char *translate_scan(struct adapter *padapter,
 			    struct wlan_network *pnetwork,
 			    char *start, char *stop)
 {
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct iw_event iwe;
 	u16 cap;
 	__le16 le_tmp;
@@ -417,7 +417,7 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 				ret = -EOPNOTSUPP;
 				goto exit;
 			}
-			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->KeyMaterial, pwep->KeyLength);
+			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);
 		}
@@ -444,8 +444,8 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 					memcpy(psta->dot118021x_UncstKey.skey,  param->u.crypt.key, min_t(u16, param->u.crypt.key_len, 16));
 
 					if (strcmp(param->u.crypt.alg, "TKIP") == 0) { /* set mic key */
-						memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
-						memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+						memcpy(psta->dot11tkiptxmickey.skey, &param->u.crypt.key[16], 8);
+						memcpy(psta->dot11tkiprxmickey.skey, &param->u.crypt.key[24], 8);
 						padapter->securitypriv.busetkipkey = false;
 					}
 
@@ -454,8 +454,8 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 					rtw_setstakey_cmd(padapter, (unsigned char *)psta, true);
 				} else { /* group key */
 					memcpy(padapter->securitypriv.dot118021XGrpKey[param->u.crypt.idx].skey,  param->u.crypt.key, min_t(u16, 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);
+					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;
 					DBG_88E(" ~~~~set sta key:groupkey\n");
 
@@ -620,7 +620,7 @@ static int rtw_wx_get_name(struct net_device *dev,
 	u32 ht_ielen = 0;
 	char *p;
 	u8 ht_cap = false;
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
 	NDIS_802_11_RATES_EX *prates = NULL;
 
@@ -669,7 +669,7 @@ static int rtw_wx_get_freq(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
 
 	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
@@ -738,7 +738,7 @@ 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);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_, (" rtw_wx_get_mode\n"));
 
@@ -938,10 +938,10 @@ static int rtw_wx_set_wap(struct net_device *dev,
 	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	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct list_head *phead;
 	u8 *dst_bssid, *src_bssid;
-	struct __queue *queue	= &(pmlmepriv->scanned_queue);
+	struct __queue *queue	= &pmlmepriv->scanned_queue;
 	struct	wlan_network	*pnetwork = NULL;
 	enum ndis_802_11_auth_mode	authmode;
 
@@ -1002,7 +1002,7 @@ static int rtw_wx_get_wap(struct net_device *dev,
 			    union iwreq_data *wrqu, char *extra)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct wlan_bssid_ex  *pcur_bss = &pmlmepriv->cur_network.network;
 
 	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
@@ -1188,8 +1188,8 @@ static int rtw_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
 {
 	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	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;
@@ -1217,7 +1217,7 @@ static int rtw_wx_get_scan(struct net_device *dev, struct iw_request_info *a,
 			break;
 	}
 
-	spin_lock_bh(&(pmlmepriv->scanned_queue.lock));
+	spin_lock_bh(&pmlmepriv->scanned_queue.lock);
 
 	phead = get_list_head(queue);
 	plist = phead->next;
@@ -1358,7 +1358,7 @@ static int rtw_wx_get_essid(struct net_device *dev,
 {
 	u32 len;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	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"));
@@ -1564,7 +1564,7 @@ static int rtw_wx_set_enc(struct net_device *dev,
 	struct ndis_802_11_wep	 wep;
 	enum ndis_802_11_auth_mode authmode;
 
-	struct iw_point *erq = &(wrqu->encoding);
+	struct iw_point *erq = &wrqu->encoding;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
@@ -1675,8 +1675,8 @@ static int rtw_wx_get_enc(struct net_device *dev,
 {
 	uint key;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct iw_point *erq = &(wrqu->encoding);
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct iw_point *erq = &wrqu->encoding;
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 
 	if (!check_fwstate(pmlmepriv, _FW_LINKED)) {
 		if (!check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
@@ -1759,7 +1759,7 @@ static int rtw_wx_set_auth(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct iw_param *param = (struct iw_param *)&(wrqu->param);
+	struct iw_param *param = (struct iw_param *)&wrqu->param;
 	int ret = 0;
 
 	switch (param->flags & IW_AUTH_INDEX) {
@@ -2012,14 +2012,9 @@ static int wpa_supplicant_ioctl(struct net_device *dev, struct iw_point *p)
 	if (!p->pointer || p->length != sizeof(struct ieee_param))
 		return -EINVAL;
 
-	param = (struct ieee_param *)rtw_malloc(p->length);
-	if (!param)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		kfree(param);
-		return -EFAULT;
-	}
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param))
+		return PTR_ERR(param);
 
 	switch (param->cmd) {
 	case IEEE_CMD_SET_WPA_PARAM:
@@ -2093,7 +2088,7 @@ static int set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid)
 	u8 keylen;
 	struct cmd_obj *pcmd;
 	struct setkey_parm *psetkeyparm;
-	struct cmd_priv	*pcmdpriv = &(padapter->cmdpriv);
+	struct cmd_priv	*pcmdpriv = &padapter->cmdpriv;
 	int res = _SUCCESS;
 
 	DBG_88E("%s\n", __func__);
@@ -2130,7 +2125,7 @@ static int set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid)
 		keylen = 16;
 	}
 
-	memcpy(&(psetkeyparm->key[0]), key, keylen);
+	memcpy(&psetkeyparm->key[0], key, keylen);
 
 	pcmd->cmdcode = _SetKey_CMD_;
 	pcmd->parmbuf = (u8 *)psetkeyparm;
@@ -2173,7 +2168,7 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 	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 security_priv *psecuritypriv = &padapter->securitypriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	DBG_88E("%s\n", __func__);
@@ -2245,7 +2240,7 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 
 			psecuritypriv->dot11PrivacyKeyIndex = wep_key_idx;
 
-			memcpy(&(psecuritypriv->dot11DefKey[wep_key_idx].skey[0]), pwep->KeyMaterial, pwep->KeyLength);
+			memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0], pwep->KeyMaterial, pwep->KeyLength);
 
 			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->KeyLength;
 
@@ -2256,7 +2251,7 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 			/* 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);
+			memcpy(&psecuritypriv->dot11DefKey[wep_key_idx].skey[0], pwep->KeyMaterial, pwep->KeyLength);
 
 			psecuritypriv->dot11DefKeylen[wep_key_idx] = pwep->KeyLength;
 
@@ -2283,8 +2278,8 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 				memcpy(psecuritypriv->dot118021XGrpKey[param->u.crypt.idx].skey,
 					    param->u.crypt.key, min_t(u16, param->u.crypt.key_len, 16));
 				/* 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);
+				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) {
@@ -2326,8 +2321,8 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 					psta->dot118021XPrivacy = _TKIP_;
 
 					/* set mic key */
-					memcpy(psta->dot11tkiptxmickey.skey, &(param->u.crypt.key[16]), 8);
-					memcpy(psta->dot11tkiprxmickey.skey, &(param->u.crypt.key[24]), 8);
+					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) {
@@ -2357,8 +2352,8 @@ static int rtw_set_encryption(struct net_device *dev, struct ieee_param *param,
 						    param->u.crypt.key, min_t(u16, param->u.crypt.key_len, 16));
 
 					/* 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);
+					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) {
@@ -2398,7 +2393,7 @@ static int rtw_set_beacon(struct net_device *dev, struct ieee_param *param, int
 {
 	int ret = 0;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	unsigned char *pbuf = param->u.bcn_ie.buf;
 
@@ -2436,7 +2431,7 @@ 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 mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	DBG_88E("rtw_add_sta(aid =%d) =%pM\n", param->u.add_sta.aid, (param->sta_addr));
@@ -2489,7 +2484,7 @@ static int rtw_del_sta(struct net_device *dev, struct ieee_param *param)
 {
 	struct sta_info *psta = NULL;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	int updated = 0;
 
@@ -2524,7 +2519,7 @@ static int rtw_ioctl_get_sta_data(struct net_device *dev, struct ieee_param *par
 	int ret = 0;
 	struct sta_info *psta = NULL;
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	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;
@@ -2580,7 +2575,7 @@ 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 mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct sta_priv *pstapriv = &padapter->stapriv;
 
 	DBG_88E("rtw_get_sta_wpaie, sta_addr: %pM\n", (param->sta_addr));
@@ -2616,8 +2611,8 @@ static int rtw_set_wps_beacon(struct net_device *dev, struct ieee_param *param,
 {
 	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);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
 	int ie_len;
 
 	DBG_88E("%s, len =%d\n", __func__, len);
@@ -2651,7 +2646,7 @@ static int rtw_set_wps_beacon(struct net_device *dev, struct ieee_param *param,
 static int rtw_set_wps_probe_resp(struct net_device *dev, struct ieee_param *param, int len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int ie_len;
 
 	DBG_88E("%s, len =%d\n", __func__, len);
@@ -2680,7 +2675,7 @@ static int rtw_set_wps_probe_resp(struct net_device *dev, struct ieee_param *par
 static int rtw_set_wps_assoc_resp(struct net_device *dev, struct ieee_param *param, int len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	int ie_len;
 
 	DBG_88E("%s, len =%d\n", __func__, len);
@@ -2710,9 +2705,9 @@ static int rtw_set_wps_assoc_resp(struct net_device *dev, struct ieee_param *par
 static int rtw_set_hidden_ssid(struct net_device *dev, struct ieee_param *param, int len)
 {
 	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 mlme_priv *pmlmepriv = &padapter->mlmepriv;
+	struct mlme_ext_priv	*pmlmeext = &padapter->mlmeextpriv;
+	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
 	u8 value;
 
@@ -2734,7 +2729,7 @@ static int rtw_set_hidden_ssid(struct net_device *dev, struct ieee_param *param,
 static int rtw_ioctl_acl_remove_sta(struct net_device *dev, struct ieee_param *param, int len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return -EINVAL;
@@ -2748,7 +2743,7 @@ static int rtw_ioctl_acl_remove_sta(struct net_device *dev, struct ieee_param *p
 static int rtw_ioctl_acl_add_sta(struct net_device *dev, struct ieee_param *param, int len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return -EINVAL;
@@ -2762,7 +2757,7 @@ static int rtw_ioctl_acl_add_sta(struct net_device *dev, struct ieee_param *para
 static int rtw_ioctl_set_macaddr_acl(struct net_device *dev, struct ieee_param *param, int len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(dev);
-	struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 
 	if (!check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return -EINVAL;
@@ -2789,14 +2784,9 @@ static int rtw_hostapd_ioctl(struct net_device *dev, struct iw_point *p)
 	if (!p->pointer || p->length != sizeof(struct ieee_param))
 		return -EINVAL;
 
-	param = (struct ieee_param *)rtw_malloc(p->length);
-	if (!param)
-		return -ENOMEM;
-
-	if (copy_from_user(param, p->pointer, p->length)) {
-		kfree(param);
-		return -EFAULT;
-	}
+	param = memdup_user(p->pointer, p->length);
+	if (IS_ERR(param))
+		return PTR_ERR(param);
 
 	switch (param->cmd) {
 	case RTL871X_HOSTAPD_FLUSH:
@@ -2882,7 +2872,7 @@ static int rtw_wx_set_priv(struct net_device *dev,
 	/* added for wps2.0 @20110524 */
 	if (dwrq->flags == 0x8766 && len > 8) {
 		u32 cp_sz;
-		struct mlme_priv *pmlmepriv = &(padapter->mlmepriv);
+		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 		u8 *probereq_wpsie = ext;
 		int probereq_wpsie_len = len;
 		u8 wps_oui[4] = {0x0, 0x50, 0xf2, 0x04};
diff --git a/drivers/staging/rtl8188eu/os_dep/rtw_android.c b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
index daf6db354982..bf86d03820ca 100644
--- a/drivers/staging/rtl8188eu/os_dep/rtw_android.c
+++ b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
@@ -77,7 +77,7 @@ static int rtw_android_get_rssi(struct net_device *net, char *command,
 				int total_len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(net);
-	struct	mlme_priv	*pmlmepriv = &(padapter->mlmepriv);
+	struct	mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct	wlan_network	*pcur_network = &pmlmepriv->cur_network;
 	int bytes_written = 0;
 
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index d3664e508cbe..a7cd4de65b28 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -81,8 +81,8 @@ static int _rtl92e_hard_start_xmit(struct sk_buff *skb, struct net_device *dev);
 static void _rtl92e_tx_cmd(struct net_device *dev, struct sk_buff *skb);
 static short _rtl92e_tx(struct net_device *dev, struct sk_buff *skb);
 static short _rtl92e_pci_initdescring(struct net_device *dev);
-static void _rtl92e_irq_tx_tasklet(struct r8192_priv *priv);
-static void _rtl92e_irq_rx_tasklet(struct r8192_priv *priv);
+static void _rtl92e_irq_tx_tasklet(unsigned long data);
+static void _rtl92e_irq_rx_tasklet(unsigned long data);
 static void _rtl92e_cancel_deferred_work(struct r8192_priv *priv);
 static int _rtl92e_up(struct net_device *dev, bool is_silent_reset);
 static int _rtl92e_try_up(struct net_device *dev);
@@ -516,8 +516,9 @@ static int _rtl92e_handle_assoc_response(struct net_device *dev,
 	return 0;
 }
 
-static void _rtl92e_prepare_beacon(struct r8192_priv *priv)
+static void _rtl92e_prepare_beacon(unsigned long data)
 {
+	struct r8192_priv *priv = (struct r8192_priv *)data;
 	struct net_device *dev = priv->rtllib->dev;
 	struct sk_buff *pskb = NULL, *pnewskb = NULL;
 	struct cb_desc *tcb_desc = NULL;
@@ -1007,14 +1008,11 @@ static void _rtl92e_init_priv_task(struct net_device *dev)
 			      (void *)rtl92e_hw_wakeup_wq, dev);
 	INIT_DELAYED_WORK_RSL(&priv->rtllib->hw_sleep_wq,
 			      (void *)rtl92e_hw_sleep_wq, dev);
-	tasklet_init(&priv->irq_rx_tasklet,
-		     (void(*)(unsigned long))_rtl92e_irq_rx_tasklet,
+	tasklet_init(&priv->irq_rx_tasklet, _rtl92e_irq_rx_tasklet,
 		     (unsigned long)priv);
-	tasklet_init(&priv->irq_tx_tasklet,
-		     (void(*)(unsigned long))_rtl92e_irq_tx_tasklet,
+	tasklet_init(&priv->irq_tx_tasklet, _rtl92e_irq_tx_tasklet,
 		     (unsigned long)priv);
-	tasklet_init(&priv->irq_prepare_beacon_tasklet,
-		     (void(*)(unsigned long))_rtl92e_prepare_beacon,
+	tasklet_init(&priv->irq_prepare_beacon_tasklet, _rtl92e_prepare_beacon,
 		     (unsigned long)priv);
 }
 
@@ -2113,13 +2111,17 @@ static void _rtl92e_tx_resume(struct net_device *dev)
 	}
 }
 
-static void _rtl92e_irq_tx_tasklet(struct r8192_priv *priv)
+static void _rtl92e_irq_tx_tasklet(unsigned long data)
 {
+	struct r8192_priv *priv = (struct r8192_priv *)data;
+
 	_rtl92e_tx_resume(priv->rtllib->dev);
 }
 
-static void _rtl92e_irq_rx_tasklet(struct r8192_priv *priv)
+static void _rtl92e_irq_rx_tasklet(unsigned long data)
 {
+	struct r8192_priv *priv = (struct r8192_priv *)data;
+
 	_rtl92e_rx_normal(priv->rtllib->dev);
 
 	rtl92e_writel(priv->rtllib->dev, INTA_MASK,
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index 20e494186c9e..462835684e8b 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -456,7 +456,7 @@ static void _rtl92e_dm_bandwidth_autoswitch(struct net_device *dev)
 	if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20 ||
 	   !priv->rtllib->bandwidth_auto_switch.bautoswitch_enable)
 		return;
-	if (priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz == false) {
+	if (!priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz) {
 		if (priv->undecorated_smoothed_pwdb <=
 		    priv->rtllib->bandwidth_auto_switch.threshold_40Mhzto20Mhz)
 			priv->rtllib->bandwidth_auto_switch.bforced_tx20Mhz = true;
@@ -1297,7 +1297,7 @@ static void _rtl92e_dm_dig_init(struct net_device *dev)
 static void _rtl92e_dm_ctrl_initgain_byrssi(struct net_device *dev)
 {
 
-	if (dm_digtable.dig_enable_flag == false)
+	if (!dm_digtable.dig_enable_flag)
 		return;
 
 	if (dm_digtable.dig_algorithm == DIG_ALGO_BY_FALSE_ALARM)
@@ -1332,7 +1332,7 @@ static void _rtl92e_dm_ctrl_initgain_byrssi_driver(struct net_device *dev)
 	u8 i;
 	static u8	fw_dig;
 
-	if (dm_digtable.dig_enable_flag == false)
+	if (!dm_digtable.dig_enable_flag)
 		return;
 
 	if (dm_digtable.dig_algorithm_switch)
@@ -1366,7 +1366,7 @@ static void _rtl92e_dm_ctrl_initgain_byrssi_false_alarm(struct net_device *dev)
 	static u32 reset_cnt;
 	u8 i;
 
-	if (dm_digtable.dig_enable_flag == false)
+	if (!dm_digtable.dig_enable_flag)
 		return;
 
 	if (dm_digtable.dig_algorithm_switch) {
@@ -1501,7 +1501,7 @@ static void _rtl92e_dm_initial_gain(struct net_device *dev)
 		reset_cnt = 0;
 	}
 
-	if (rtllib_act_scanning(priv->rtllib, true) == true) {
+	if (rtllib_act_scanning(priv->rtllib, true)) {
 		force_write = 1;
 		return;
 	}
@@ -2444,7 +2444,7 @@ static void _rtl92e_dm_init_dynamic_tx_power(struct net_device *dev)
 static void _rtl92e_dm_dynamic_tx_power(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	unsigned int txhipower_threshhold = 0;
+	unsigned int txhipower_threshold = 0;
 	unsigned int txlowpower_threshold = 0;
 
 	if (priv->rtllib->bdynamic_txpower_enable != true) {
@@ -2454,10 +2454,10 @@ static void _rtl92e_dm_dynamic_tx_power(struct net_device *dev)
 	}
 	if ((priv->rtllib->pHTInfo->IOTPeer == HT_IOT_PEER_ATHEROS) &&
 	    (priv->rtllib->mode == IEEE_G)) {
-		txhipower_threshhold = TX_POWER_ATHEROAP_THRESH_HIGH;
+		txhipower_threshold = TX_POWER_ATHEROAP_THRESH_HIGH;
 		txlowpower_threshold = TX_POWER_ATHEROAP_THRESH_LOW;
 	} else {
-		txhipower_threshhold = TX_POWER_NEAR_FIELD_THRESH_HIGH;
+		txhipower_threshold = TX_POWER_NEAR_FIELD_THRESH_HIGH;
 		txlowpower_threshold = TX_POWER_NEAR_FIELD_THRESH_LOW;
 	}
 
@@ -2465,7 +2465,7 @@ static void _rtl92e_dm_dynamic_tx_power(struct net_device *dev)
 		 priv->undecorated_smoothed_pwdb);
 
 	if (priv->rtllib->state == RTLLIB_LINKED) {
-		if (priv->undecorated_smoothed_pwdb >= txhipower_threshhold) {
+		if (priv->undecorated_smoothed_pwdb >= txhipower_threshold) {
 			priv->bDynamicTxHighPower = true;
 			priv->bDynamicTxLowPower = false;
 		} else {
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index d83d72594312..8abc921ecb3e 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -371,7 +371,7 @@ void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
 	if ((ieee->iw_mode == IW_MODE_ADHOC) ||
 	    (ieee->iw_mode == IW_MODE_MASTER)) {
 		pHTInfoEle->ControlChl	= ieee->current_network.channel;
-		pHTInfoEle->ExtChlOffset = ((pHT->bRegBW40MHz == false) ?
+		pHTInfoEle->ExtChlOffset = ((!pHT->bRegBW40MHz) ?
 					    HT_EXTCHNL_OFFSET_NO_EXT :
 					    (ieee->current_network.channel <= 6)
 					    ? HT_EXTCHNL_OFFSET_UPPER :
@@ -526,7 +526,7 @@ void HTOnAssocRsp(struct rtllib_device *ieee)
 	static u8 EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
 	static u8 EWC11NHTInfo[] = {0x00, 0x90, 0x4c, 0x34};
 
-	if (pHTInfo->bCurrentHTSupport == false) {
+	if (!pHTInfo->bCurrentHTSupport) {
 		netdev_warn(ieee->dev, "%s(): HT_DISABLE\n", __func__);
 		return;
 	}
@@ -873,7 +873,7 @@ void HTSetConnectBwMode(struct rtllib_device *ieee,
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 
-	if (pHTInfo->bRegBW40MHz == false)
+	if (!pHTInfo->bRegBW40MHz)
 		return;
 
 	if (ieee->GetHalfNmodeSupportByAPsHandler(ieee->dev))
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index e101f7b13c7e..195d963c4fbb 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -520,55 +520,68 @@ static bool AddReorderEntry(struct rx_ts_record *pTS, struct rx_reorder_entry *p
 	return true;
 }
 
-void ieee80211_indicate_packets(struct ieee80211_device *ieee, struct ieee80211_rxb **prxbIndicateArray, u8  index)
+static void indicate_packets(struct ieee80211_device *ieee,
+			     struct ieee80211_rxb *rxb)
 {
-	u8 i = 0, j = 0;
+	struct net_device_stats *stats = &ieee->stats;
+	struct net_device *dev = ieee->dev;
 	u16 ethertype;
-//	if(index > 1)
-//		IEEE80211_DEBUG(IEEE80211_DL_REORDER,"%s(): hahahahhhh, We indicate packet from reorder list, index is %u\n",__func__,index);
-	for (j = 0; j < index; j++) {
-//added by amy for reorder
-		struct ieee80211_rxb *prxb = prxbIndicateArray[j];
-		for (i = 0; i < prxb->nr_subframes; i++) {
-			struct sk_buff *sub_skb = prxb->subframes[i];
+	u8 i;
+
+	for (i = 0; i < rxb->nr_subframes; i++) {
+		struct sk_buff *sub_skb = rxb->subframes[i];
+
+		if (!sub_skb)
+			continue;
 
 		/* convert hdr + possible LLC headers into Ethernet header */
-			ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
-			if (sub_skb->len >= 8 &&
-				((memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) == 0 &&
-				  ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
-				 memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
+		ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
+		if (sub_skb->len >= 8 &&
+		    ((!memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) &&
+			ethertype != ETH_P_AARP &&
+			ethertype != ETH_P_IPX) ||
+		     !memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE))) {
 			/* remove RFC1042 or Bridge-Tunnel encapsulation and
 			 * replace EtherType */
-				skb_pull(sub_skb, SNAP_SIZE);
-				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
-				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
-			} else {
+			skb_pull(sub_skb, SNAP_SIZE);
+		} else {
 			/* Leave Ethernet header part of hdr and full payload */
-				put_unaligned_be16(sub_skb->len, skb_push(sub_skb, 2));
-				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->src, ETH_ALEN);
-				memcpy(skb_push(sub_skb, ETH_ALEN), prxb->dst, ETH_ALEN);
-			}
-			//stats->rx_packets++;
-			//stats->rx_bytes += sub_skb->len;
+			put_unaligned_be16(sub_skb->len, skb_push(sub_skb, 2));
+		}
+		memcpy(skb_push(sub_skb, ETH_ALEN), rxb->src, ETH_ALEN);
+		memcpy(skb_push(sub_skb, ETH_ALEN), rxb->dst, ETH_ALEN);
+
+		stats->rx_packets++;
+		stats->rx_bytes += sub_skb->len;
+		if (is_multicast_ether_addr(rxb->dst))
+			stats->multicast++;
 
 		/* Indicate the packets to upper layer */
-			if (sub_skb) {
-				sub_skb->protocol = eth_type_trans(sub_skb, ieee->dev);
-				memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
-				sub_skb->dev = ieee->dev;
-				sub_skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
-				//skb->ip_summed = CHECKSUM_UNNECESSARY; /* 802.11 crc not sufficient */
-				ieee->last_rx_ps_time = jiffies;
-				netif_rx(sub_skb);
-			}
-		}
+		sub_skb->protocol = eth_type_trans(sub_skb, dev);
+		memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
+		sub_skb->dev = dev;
+		/* 802.11 crc not sufficient */
+		sub_skb->ip_summed = CHECKSUM_NONE;
+		ieee->last_rx_ps_time = jiffies;
+		netif_rx(sub_skb);
+	}
+}
+
+void ieee80211_indicate_packets(struct ieee80211_device *ieee,
+				struct ieee80211_rxb **prxbIndicateArray,
+				u8 index)
+{
+	u8 i;
+
+	for (i = 0; i < index; i++) {
+		struct ieee80211_rxb *prxb = prxbIndicateArray[i];
+
+		indicate_packets(ieee, prxb);
 		kfree(prxb);
 		prxb = NULL;
 	}
 }
 
-
 static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 				    struct ieee80211_rxb *prxb,
 				    struct rx_ts_record *pTS, u16 SeqNum)
@@ -877,7 +890,6 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	u16 fc, type, stype, sc;
 	struct net_device_stats *stats;
 	unsigned int frag;
-	u16 ethertype;
 	//added by amy for reorder
 	u8	TID = 0;
 	u16	SeqNum = 0;
@@ -1260,47 +1272,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 
 //added by amy for reorder
 	if (!ieee->pHTInfo->bCurRxReorderEnable || !pTS) {
-//added by amy for reorder
-		for (i = 0; i < rxb->nr_subframes; i++) {
-			struct sk_buff *sub_skb = rxb->subframes[i];
-
-			if (sub_skb) {
-				/* convert hdr + possible LLC headers into Ethernet header */
-				ethertype = (sub_skb->data[6] << 8) | sub_skb->data[7];
-				if (sub_skb->len >= 8 &&
-						((memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) == 0 &&
-						  ethertype != ETH_P_AARP && ethertype != ETH_P_IPX) ||
-						 memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE) == 0)) {
-					/* remove RFC1042 or Bridge-Tunnel encapsulation and
-					 * replace EtherType */
-					skb_pull(sub_skb, SNAP_SIZE);
-					memcpy(skb_push(sub_skb, ETH_ALEN), src, ETH_ALEN);
-					memcpy(skb_push(sub_skb, ETH_ALEN), dst, ETH_ALEN);
-				} else {
-					u16 len;
-					/* Leave Ethernet header part of hdr and full payload */
-					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);
-				}
-
-				stats->rx_packets++;
-				stats->rx_bytes += sub_skb->len;
-				if (is_multicast_ether_addr(dst)) {
-					stats->multicast++;
-				}
-
-				/* Indicate the packets to upper layer */
-				sub_skb->protocol = eth_type_trans(sub_skb, dev);
-				memset(sub_skb->cb, 0, sizeof(sub_skb->cb));
-				sub_skb->dev = dev;
-				sub_skb->ip_summed = CHECKSUM_NONE; /* 802.11 crc not sufficient */
-				//skb->ip_summed = CHECKSUM_UNNECESSARY; /* 802.11 crc not sufficient */
-				ieee->last_rx_ps_time = jiffies;
-				netif_rx(sub_skb);
-			}
-		}
+		indicate_packets(ieee, rxb);
 		kfree(rxb);
 		rxb = NULL;
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 0ee054d82832..63a561ab4a76 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -372,9 +372,9 @@ ieee80211_query_HTCapShortGI(struct ieee80211_device *ieee, struct cb_desc *tcb_
 		return;
 	}
 
-	if ((pHTInfo->bCurBW40MHz == true) && pHTInfo->bCurShortGI40MHz)
+	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI40MHz)
 		tcb_desc->bUseShortGI = true;
-	else if ((pHTInfo->bCurBW40MHz == false) && pHTInfo->bCurShortGI20MHz)
+	else if (!pHTInfo->bCurBW40MHz && pHTInfo->bCurShortGI20MHz)
 		tcb_desc->bUseShortGI = true;
 }
 
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index f0b85338b567..2f0d0ffa6fae 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -71,12 +71,13 @@ int _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	_init_queue(&pxmitpriv->apsd_queue);
 	_init_queue(&pxmitpriv->free_xmit_queue);
 	/*
-	 * Please allocate memory with the sz = (struct xmit_frame) * NR_XMITFRAME,
+	 * Please allocate memory with sz = (struct xmit_frame) * NR_XMITFRAME,
 	 * and initialize free_xmit_frame below.
 	 * Please also apply  free_txobj to link_up all the xmit_frames...
 	 */
 	pxmitpriv->pallocated_frame_buf =
-		kmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4, GFP_ATOMIC);
+		kmalloc(NR_XMITFRAME * sizeof(struct xmit_frame) + 4,
+			GFP_ATOMIC);
 	if (!pxmitpriv->pallocated_frame_buf) {
 		pxmitpriv->pxmit_frame_buf = NULL;
 		return -ENOMEM;
@@ -126,8 +127,8 @@ int _r8712_init_xmit_priv(struct xmit_priv *pxmitpriv,
 	pxmitbuf = (struct xmit_buf *)pxmitpriv->pxmitbuf;
 	for (i = 0; i < NR_XMITBUFF; i++) {
 		INIT_LIST_HEAD(&pxmitbuf->list);
-		pxmitbuf->pallocated_buf = kmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ,
-						   GFP_ATOMIC);
+		pxmitbuf->pallocated_buf =
+			kmalloc(MAX_XMITBUF_SZ + XMITBUF_ALIGN_SZ, GFP_ATOMIC);
 		if (!pxmitbuf->pallocated_buf)
 			return -ENOMEM;
 		pxmitbuf->pbuf = pxmitbuf->pallocated_buf + XMITBUF_ALIGN_SZ -
@@ -350,7 +351,7 @@ static int xmitframe_addmic(struct _adapter *padapter,
 	struct	sta_info *stainfo;
 	struct	qos_priv *pqospriv = &(padapter->mlmepriv.qospriv);
 	struct	pkt_attrib  *pattrib = &pxmitframe->attrib;
-	struct	security_priv *psecuritypriv = &padapter->securitypriv;
+	struct	security_priv *psecpriv = &padapter->securitypriv;
 	struct	xmit_priv *pxmitpriv = &padapter->xmitpriv;
 	u8 priority[4] = {0x0, 0x0, 0x0, 0x0};
 	bool bmcst = is_multicast_ether_addr(pattrib->ra);
@@ -368,15 +369,14 @@ static int xmitframe_addmic(struct _adapter *padapter,
 					   0x0, 0x0};
 			pframe = pxmitframe->buf_addr + TXDESC_OFFSET;
 			if (bmcst) {
-				if (!memcmp(psecuritypriv->XGrptxmickey
-				   [psecuritypriv->XGrpKeyid].skey,
+				if (!memcmp(psecpriv->XGrptxmickey
+				   [psecpriv->XGrpKeyid].skey,
 				   null_key, 16))
 					return -ENOMEM;
 				/*start to calculate the mic code*/
 				r8712_secmicsetkey(&micdata,
-					 psecuritypriv->
-					 XGrptxmickey[psecuritypriv->
-					XGrpKeyid].skey);
+					psecpriv->XGrptxmickey
+					[psecpriv->XGrpKeyid].skey);
 			} else {
 				if (!memcmp(&stainfo->tkiptxmickey.skey[0],
 					    null_key, 16))
@@ -416,7 +416,7 @@ static int xmitframe_addmic(struct _adapter *padapter,
 					length = pattrib->last_txcmdsz -
 						  pattrib->hdrlen -
 						  pattrib->iv_len -
-						  ((psecuritypriv->sw_encrypt)
+						  ((psecpriv->sw_encrypt)
 						  ? pattrib->icv_len : 0);
 					r8712_secmicappend(&micdata, payload,
 							   length);
@@ -424,7 +424,7 @@ static int xmitframe_addmic(struct _adapter *padapter,
 				} else {
 					length = pxmitpriv->frag_len -
 					    pattrib->hdrlen - pattrib->iv_len -
-					    ((psecuritypriv->sw_encrypt) ?
+					    ((psecpriv->sw_encrypt) ?
 					    pattrib->icv_len : 0);
 					r8712_secmicappend(&micdata, payload,
 							   length);
@@ -477,75 +477,72 @@ static int make_wlanhdr(struct _adapter *padapter, u8 *hdr,
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	__le16 *fctrl = &pwlanhdr->frame_ctl;
+	u8 *bssid;
 
 	memset(hdr, 0, WLANHDR_OFFSET);
 	SetFrameSubType(fctrl, pattrib->subtype);
-	if (pattrib->subtype & WIFI_DATA_TYPE) {
-		if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
-			/* to_ds = 1, fr_ds = 0; */
-			SetToDs(fctrl);
-			memcpy(pwlanhdr->addr1, get_bssid(pmlmepriv),
-				ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
-		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
-			/* to_ds = 0, fr_ds = 1; */
-			SetFrDs(fctrl);
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, get_bssid(pmlmepriv),
-				ETH_ALEN);
-			memcpy(pwlanhdr->addr3, pattrib->src, ETH_ALEN);
-		} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
-			   check_fwstate(pmlmepriv,
-					 WIFI_ADHOC_MASTER_STATE)) {
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv),
-				ETH_ALEN);
-		} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
-			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
-			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
-			memcpy(pwlanhdr->addr3, get_bssid(pmlmepriv),
-				ETH_ALEN);
-		} else {
-			return -EINVAL;
-		}
+	if (!(pattrib->subtype & WIFI_DATA_TYPE))
+		return 0;
 
-		if (pattrib->encrypt)
-			SetPrivacy(fctrl);
-		if (pqospriv->qos_option) {
-			qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
-			if (pattrib->priority)
-				SetPriority(qc, pattrib->priority);
-			SetAckpolicy(qc, pattrib->ack_policy);
-		}
-		/* TODO: fill HT Control Field */
-		/* Update Seq Num will be handled by f/w */
-		{
-			struct sta_info *psta;
-			bool bmcst = is_multicast_ether_addr(pattrib->ra);
-
-			if (pattrib->psta) {
-				psta = pattrib->psta;
-			} else {
-				if (bmcst)
-					psta = r8712_get_bcmc_stainfo(padapter);
-				else
-					psta =
-					 r8712_get_stainfo(&padapter->stapriv,
-					 pattrib->ra);
-			}
-			if (psta) {
-				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);
-			}
+	bssid = get_bssid(pmlmepriv);
+
+	if (check_fwstate(pmlmepriv,  WIFI_STATION_STATE)) {
+		/* to_ds = 1, fr_ds = 0; */
+		SetToDs(fctrl);
+		ether_addr_copy(pwlanhdr->addr1, bssid);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, pattrib->dst);
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+		/* to_ds = 0, fr_ds = 1; */
+		SetFrDs(fctrl);
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, bssid);
+		ether_addr_copy(pwlanhdr->addr3, pattrib->src);
+	} else if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) ||
+		   check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE)) {
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, bssid);
+	} else if (check_fwstate(pmlmepriv, WIFI_MP_STATE)) {
+		ether_addr_copy(pwlanhdr->addr1, pattrib->dst);
+		ether_addr_copy(pwlanhdr->addr2, pattrib->src);
+		ether_addr_copy(pwlanhdr->addr3, bssid);
+	} else {
+		return -EINVAL;
+	}
+
+	if (pattrib->encrypt)
+		SetPrivacy(fctrl);
+	if (pqospriv->qos_option) {
+		qc = (unsigned short *)(hdr + pattrib->hdrlen - 2);
+		if (pattrib->priority)
+			SetPriority(qc, pattrib->priority);
+		SetAckpolicy(qc, pattrib->ack_policy);
+	}
+	/* TODO: fill HT Control Field */
+	/* Update Seq Num will be handled by f/w */
+	{
+		struct sta_info *psta;
+		bool bmcst = is_multicast_ether_addr(pattrib->ra);
+
+		if (pattrib->psta)
+			psta = pattrib->psta;
+		else if (bmcst)
+			psta = r8712_get_bcmc_stainfo(padapter);
+		else
+			psta = r8712_get_stainfo(&padapter->stapriv,
+						 pattrib->ra);
+
+		if (psta) {
+			u16 *txtid = psta->sta_xmitpriv.txseq_tid;
+
+			txtid[pattrib->priority]++;
+			txtid[pattrib->priority] &= 0xFFF;
+			pattrib->seqnum = txtid[pattrib->priority];
+			SetSeqNum(hdr, pattrib->seqnum);
 		}
 	}
+
 	return 0;
 }
 
@@ -589,7 +586,7 @@ sint r8712_xmitframe_coalesce(struct _adapter *padapter, _pkt *pkt,
 	addr_t addr;
 	u8 *pframe, *mem_start, *ptxdesc;
 	struct sta_info		*psta;
-	struct security_priv	*psecuritypriv = &padapter->securitypriv;
+	struct security_priv	*psecpriv = &padapter->securitypriv;
 	struct mlme_priv	*pmlmepriv = &padapter->mlmepriv;
 	struct xmit_priv	*pxmitpriv = &padapter->xmitpriv;
 	struct pkt_attrib	*pattrib = &pxmitframe->attrib;
@@ -632,15 +629,13 @@ sint r8712_xmitframe_coalesce(struct _adapter *padapter, _pkt *pkt,
 				case _WEP40_:
 				case _WEP104_:
 					WEP_IV(pattrib->iv, psta->txpn,
-					       (u8)psecuritypriv->
-					       PrivacyKeyIndex);
+					       (u8)psecpriv->PrivacyKeyIndex);
 					break;
 				case _TKIP_:
 					if (bmcst)
 						TKIP_IV(pattrib->iv,
 						    psta->txpn,
-						    (u8)psecuritypriv->
-						    XGrpKeyid);
+						    (u8)psecpriv->XGrpKeyid);
 					else
 						TKIP_IV(pattrib->iv, psta->txpn,
 							0);
@@ -648,8 +643,7 @@ sint r8712_xmitframe_coalesce(struct _adapter *padapter, _pkt *pkt,
 				case _AES_:
 					if (bmcst)
 						AES_IV(pattrib->iv, psta->txpn,
-						    (u8)psecuritypriv->
-						    XGrpKeyid);
+						    (u8)psecpriv->XGrpKeyid);
 					else
 						AES_IV(pattrib->iv, psta->txpn,
 						       0);
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.h b/drivers/staging/rtl8712/rtl871x_xmit.h
index f227828094bf..c0c0c781fe17 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.h
+++ b/drivers/staging/rtl8712/rtl871x_xmit.h
@@ -115,7 +115,7 @@ struct pkt_attrib {
 	u8	icv_len;
 	unsigned char iv[8];
 	unsigned char icv[8];
-	u8	dst[ETH_ALEN];
+	u8	dst[ETH_ALEN] __aligned(2);	/* for ether_addr_copy */
 	u8	src[ETH_ALEN];
 	u8	ta[ETH_ALEN];
 	u8	ra[ETH_ALEN];
diff --git a/drivers/staging/rtl8712/usb_halinit.c b/drivers/staging/rtl8712/usb_halinit.c
index 6cc4a704c3a0..313c569748e9 100644
--- a/drivers/staging/rtl8712/usb_halinit.c
+++ b/drivers/staging/rtl8712/usb_halinit.c
@@ -58,7 +58,7 @@ u8 r8712_usb_hal_bus_init(struct _adapter *adapter)
 		r8712_write8(adapter, SYS_ISO_CTRL + 1, val8);
 		val8 = r8712_read8(adapter, SYS_ISO_CTRL + 1);
 		val8 = val8 & 0xEF;
-		/* attatch AFE PLL to MACTOP/BB/PCIe Digital */
+		/* attach AFE PLL to MACTOP/BB/PCIe Digital */
 		r8712_write8(adapter, SYS_ISO_CTRL + 1, val8);
 		val8 = r8712_read8(adapter, AFE_XTAL_CTRL + 1);
 		val8 = val8 & 0xFB;
diff --git a/drivers/staging/rtl8712/wifi.h b/drivers/staging/rtl8712/wifi.h
index be731f1a2209..91b65731fcaa 100644
--- a/drivers/staging/rtl8712/wifi.h
+++ b/drivers/staging/rtl8712/wifi.h
@@ -440,7 +440,7 @@ static inline unsigned char *get_hdr_bssid(unsigned char *pframe)
 /* block-ack parameters */
 #define IEEE80211_ADDBA_PARAM_POLICY_MASK 0x0002
 #define IEEE80211_ADDBA_PARAM_TID_MASK 0x003C
-#define IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK 0xFFA0
+#define IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK 0xFFC0
 #define IEEE80211_DELBA_PARAM_TID_MASK 0xF000
 #define IEEE80211_DELBA_PARAM_INITIATOR_MASK 0x0800
 
@@ -532,13 +532,6 @@ struct ieee80211_ht_addt_info {
 #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 IEEE80211_ADDBA_PARAM_BUF_SIZE_MASK 0xFFA0
-#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)
diff --git a/drivers/staging/rtl8723bs/core/rtw_cmd.c b/drivers/staging/rtl8723bs/core/rtw_cmd.c
index efb5135ad743..bd18d1803e27 100644
--- a/drivers/staging/rtl8723bs/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723bs/core/rtw_cmd.c
@@ -822,7 +822,7 @@ u8 rtw_joinbss_cmd(struct adapter  *padapter, struct wlan_network *pnetwork)
 
 	psecnetwork->IELength = 0;
 	/*  Added by Albert 2009/02/18 */
-	/*  If the the driver wants to use the bssid to create the connection. */
+	/*  If the driver wants to use the bssid to create the connection. */
 	/*  If not,  we have to copy the connecting AP's MAC address to it so that */
 	/*  the driver just has the bssid information for PMKIDList searching. */
 
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme.c b/drivers/staging/rtl8723bs/core/rtw_mlme.c
index d7a58af76ea0..e65c5a870b46 100644
--- a/drivers/staging/rtl8723bs/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c
@@ -1097,9 +1097,6 @@ inline void rtw_indicate_scan_done(struct adapter *padapter, bool aborted)
 	    (!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);
 	}
@@ -2917,12 +2914,11 @@ 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);
+	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)
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
index 8f9da1d49343..d6d7198dfe45 100644
--- a/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
@@ -1084,7 +1084,7 @@ auth_fail:
 
 unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_frame)
 {
-	unsigned int	seq, len, status, algthm, offset;
+	unsigned int	seq, len, status, offset;
 	unsigned char *p;
 	unsigned int	go2asoc = 0;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -1103,7 +1103,6 @@ unsigned int OnAuthClient(struct adapter *padapter, union recv_frame *precv_fram
 
 	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));
 
@@ -1170,7 +1169,7 @@ authclnt_fail:
 
 unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
 {
-	u16 capab_info, listen_interval;
+	u16 capab_info;
 	struct rtw_ieee802_11_elems elems;
 	struct sta_info *pstat;
 	unsigned char 	reassoc, *p, *pos, *wpa_ie;
@@ -1216,8 +1215,6 @@ unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
 
 	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 - (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
 	pos = pframe + (sizeof(struct ieee80211_hdr_3addr) + ie_offset);
diff --git a/drivers/staging/rtl8723bs/core/rtw_recv.c b/drivers/staging/rtl8723bs/core/rtw_recv.c
index 5245098b9ecf..7e1da0e35812 100644
--- a/drivers/staging/rtl8723bs/core/rtw_recv.c
+++ b/drivers/staging/rtl8723bs/core/rtw_recv.c
@@ -10,14 +10,11 @@
 #include <rtw_debug.h>
 #include <linux/jiffies.h>
 #include <rtw_recv.h>
+#include <net/cfg80211.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 };
-
 static void rtw_signal_stat_timer_hdl(struct timer_list *t);
 
 void _rtw_init_sta_recv_priv(struct sta_recv_priv *psta_recvpriv)
@@ -1625,11 +1622,11 @@ sint wlanhdr_to_ethhdr(union recv_frame *precvframe)
 	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) &&
+	if ((!memcmp(psnap, rfc1042_header, SNAP_SIZE) &&
 		(memcmp(psnap_type, SNAP_ETH_TYPE_IPX, 2)) &&
 		(memcmp(psnap_type, SNAP_ETH_TYPE_APPLETALK_AARP, 2))) ||
 		/* eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) || */
-		 !memcmp(psnap, rtw_bridge_tunnel_header, SNAP_SIZE)) {
+		 !memcmp(psnap, bridge_tunnel_header, SNAP_SIZE)) {
 		/* remove RFC1042 or Bridge-Tunnel encapsulation and replace EtherType */
 		bsnaphdr = true;
 	} else
diff --git a/drivers/staging/rtl8723bs/core/rtw_security.c b/drivers/staging/rtl8723bs/core/rtw_security.c
index 5ebf691bd743..0f95009a30b6 100644
--- a/drivers/staging/rtl8723bs/core/rtw_security.c
+++ b/drivers/staging/rtl8723bs/core/rtw_security.c
@@ -756,7 +756,7 @@ u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe)
 				static u32 no_gkey_bc_cnt;
 				static u32 no_gkey_mc_cnt;
 
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (!psecuritypriv->binstallGrpkey) {
 					res = _FAIL;
 
 					if (start == 0)
@@ -1837,7 +1837,7 @@ u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
 				static u32 no_gkey_bc_cnt;
 				static u32 no_gkey_mc_cnt;
 
-				if (psecuritypriv->binstallGrpkey == false) {
+				if (!psecuritypriv->binstallGrpkey) {
 					res = _FAIL;
 
 					if (start == 0)
@@ -2369,7 +2369,7 @@ 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) {
+	if (securitypriv->btkip_countermeasure) {
 		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",
diff --git a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
index 09d2ca30d653..e3f56c6cc882 100644
--- a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
@@ -553,7 +553,6 @@ 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};
 
@@ -571,7 +570,6 @@ u32 rtw_init_bcmc_stainfo(struct adapter *padapter)
 	/*  default broadcast & multicast use macid 1 */
 	psta->mac_id = 1;
 
-	ptxservq = &(psta->sta_xmitpriv.be_q);
 exit:
 	return _SUCCESS;
 }
diff --git a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
index 110338dbe372..69bcd172b298 100644
--- a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
@@ -1271,13 +1271,13 @@ int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len)
 	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;
+	int ssid_len;
 
 	if (is_client_associated_to_ap(Adapter) == false)
 		return true;
@@ -1370,21 +1370,15 @@ int rtw_check_bcn_info(struct adapter *Adapter, u8 *pframe, u32 packet_len)
 	}
 
 	/* checking SSID */
+	ssid_len = 0;
 	p = rtw_get_ie(bssid->IEs + _FIXED_IE_LENGTH_, _SSID_IE_, &len, bssid->IELength - _FIXED_IE_LENGTH_);
-	if (!p) {
-		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';
+	if (p) {
+		ssid_len = *(p + 1);
+		if (ssid_len > NDIS_802_11_LENGTH_SSID)
+			ssid_len = 0;
 	}
+	memcpy(bssid->Ssid.Ssid, (p + 2), ssid_len);
+	bssid->Ssid.SsidLength = ssid_len;
 
 	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,
diff --git a/drivers/staging/rtl8723bs/hal/hal_btcoex.c b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
index d5793e4614bf..3705a60a0546 100644
--- a/drivers/staging/rtl8723bs/hal/hal_btcoex.c
+++ b/drivers/staging/rtl8723bs/hal/hal_btcoex.c
@@ -12,49 +12,6 @@
 #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;
@@ -450,7 +407,7 @@ static u8 halbtcoutsrc_Get(void *pBtcContext, u8 getType, void *pOutBuf)
 		break;
 
 	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
-		*pu8 = padapter->securitypriv.dot11PrivacyAlgrthm == 0 ? false : true;
+		*pu8 = padapter->securitypriv.dot11PrivacyAlgrthm != 0;
 		break;
 
 	case BTC_GET_BL_WIFI_UNDER_B_MODE:
diff --git a/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c b/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
index 767e2a784f78..10250642d30a 100644
--- a/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
+++ b/drivers/staging/rtl8723bs/hal/hal_com_phycfg.c
@@ -1816,7 +1816,7 @@ static void phy_CrossReferenceHTAndVHTTxPowerLimit(struct adapter *padapter)
 	s8 tempPwrLmt = 0;
 
 	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
-		for (bw = 0; bw < MAX_5G_BANDWITH_NUM; ++bw) {
+		for (bw = 0; bw < MAX_5G_BANDWIDTH_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];
@@ -1877,7 +1877,7 @@ void PHY_ConvertTxPowerLimitToPowerIndex(struct adapter *Adapter)
 	phy_CrossReferenceHTAndVHTTxPowerLimit(Adapter);
 
 	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
-		for (bw = 0; bw < MAX_2_4G_BANDWITH_NUM; ++bw) {
+		for (bw = 0; bw < MAX_2_4G_BANDWIDTH_NUM; ++bw) {
 			for (channel = 0; channel < CHANNEL_MAX_NUMBER_2G; ++channel) {
 				for (rateSection = 0; rateSection < MAX_RATE_SECTION_NUM; ++rateSection) {
 					tempPwrLmt = pHalData->TxPwrLimit_2_4G[regulation][bw][rateSection][channel][ODM_RF_PATH_A];
@@ -1920,7 +1920,7 @@ void PHY_InitTxPowerLimit(struct adapter *Adapter)
 	/* DBG_871X("=====> PHY_InitTxPowerLimit()!\n"); */
 
 	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
-		for (j = 0; j < MAX_2_4G_BANDWITH_NUM; ++j)
+		for (j = 0; j < MAX_2_4G_BANDWIDTH_NUM; ++j)
 			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
 				for (m = 0; m < CHANNEL_MAX_NUMBER_2G; ++m)
 					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
@@ -1928,7 +1928,7 @@ void PHY_InitTxPowerLimit(struct adapter *Adapter)
 	}
 
 	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
-		for (j = 0; j < MAX_5G_BANDWITH_NUM; ++j)
+		for (j = 0; j < MAX_5G_BANDWIDTH_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)
diff --git a/drivers/staging/rtl8723bs/hal/odm.c b/drivers/staging/rtl8723bs/hal/odm.c
index aa6631ee4ea7..f2a9e95a1563 100644
--- a/drivers/staging/rtl8723bs/hal/odm.c
+++ b/drivers/staging/rtl8723bs/hal/odm.c
@@ -7,19 +7,6 @@
 
 #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] = {
diff --git a/drivers/staging/rtl8723bs/hal/odm.h b/drivers/staging/rtl8723bs/hal/odm.h
index b77d1fe33a28..16e8f66a3171 100644
--- a/drivers/staging/rtl8723bs/hal/odm.h
+++ b/drivers/staging/rtl8723bs/hal/odm.h
@@ -541,7 +541,7 @@ typedef enum tag_Operation_Mode_Definition {
 
 /*  ODM_CMNINFO_WM_MODE */
 typedef enum tag_Wireless_Mode_Definition {
-	ODM_WM_UNKNOW     = 0x0,
+	ODM_WM_UNKNOWN    = 0x0,
 	ODM_WM_B          = BIT0,
 	ODM_WM_G          = BIT1,
 	ODM_WM_A          = BIT2,
diff --git a/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h b/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
index f2c0707aad4c..1c6c08000e27 100644
--- a/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
+++ b/drivers/staging/rtl8723bs/hal/odm_RegDefine11N.h
@@ -31,8 +31,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/rtl8723bs/hal/rtl8723bs_recv.c b/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
index c3051ebaeb78..29c29e2e125b 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723bs_recv.c
@@ -311,39 +311,21 @@ static void rtl8723bs_recv_tasklet(unsigned long priv)
 				}
 
 				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;
-					}
+				if (!pkt_copy) {
+					DBG_8192C("%s: alloc_skb fail, drop frame\n", __func__);
+					rtw_free_recvframe(precvframe, &precvpriv->free_recv_queue);
+					break;
 				}
 
+				pkt_copy->dev = padapter->pnetdev;
+				precvframe->u.hdr.pkt = pkt_copy;
+				skb_reserve(pkt_copy, 8 - ((SIZE_PTR)(pkt_copy->data) & 7));/* force pkt_copy->data at 8-byte alignment address */
+				skb_reserve(pkt_copy, shift_sz);/* force ip_hdr at 8-byte alignment address according to shift_sz. */
+				memcpy(pkt_copy->data, (ptr + rx_report_sz + pattrib->shift_sz), skb_len);
+				precvframe->u.hdr.rx_head = pkt_copy->head;
+				precvframe->u.hdr.rx_data = precvframe->u.hdr.rx_tail = pkt_copy->data;
+				precvframe->u.hdr.rx_end = skb_end_pointer(pkt_copy);
+
 				recvframe_put(precvframe, skb_len);
 				/* recvframe_pull(precvframe, drvinfo_sz + RXDESC_SIZE); */
 
diff --git a/drivers/staging/rtl8723bs/hal/sdio_halinit.c b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
index 7853af53051d..e42d8c18e1ae 100644
--- a/drivers/staging/rtl8723bs/hal/sdio_halinit.c
+++ b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
@@ -544,13 +544,9 @@ static void _InitRetryFunction(struct adapter *padapter)
 
 static void HalRxAggr8723BSdio(struct adapter *padapter)
 {
-	struct registry_priv *pregistrypriv;
 	u8 valueDMATimeout;
 	u8 valueDMAPageCount;
 
-
-	pregistrypriv = &padapter->registrypriv;
-
 	valueDMATimeout = 0x06;
 	valueDMAPageCount = 0x06;
 
diff --git a/drivers/staging/rtl8723bs/include/hal_data.h b/drivers/staging/rtl8723bs/include/hal_data.h
index e5e667df6154..fa5d70016f05 100644
--- a/drivers/staging/rtl8723bs/include/hal_data.h
+++ b/drivers/staging/rtl8723bs/include/hal_data.h
@@ -56,9 +56,9 @@ enum RT_AMPDU_BURST {
 /*  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_2_4G_BANDWIDTH_NUM			4
 #define MAX_RATE_SECTION_NUM			10
-#define MAX_5G_BANDWITH_NUM			4
+#define MAX_5G_BANDWIDTH_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 */
@@ -280,14 +280,14 @@ struct hal_com_data {
 
 	/*  Power Limit Table for 2.4G */
 	s8	TxPwrLimit_2_4G[MAX_REGULATION_NUM]
-						[MAX_2_4G_BANDWITH_NUM]
+						[MAX_2_4G_BANDWIDTH_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_5G_BANDWIDTH_NUM]
 						[MAX_RATE_SECTION_NUM]
 						[CHANNEL_MAX_NUMBER_5G]
 						[MAX_RF_PATH_NUM];
diff --git a/drivers/staging/rtl8723bs/include/rtw_recv.h b/drivers/staging/rtl8723bs/include/rtw_recv.h
index 98c3e92245b7..a851b818ef0e 100644
--- a/drivers/staging/rtl8723bs/include/rtw_recv.h
+++ b/drivers/staging/rtl8723bs/include/rtw_recv.h
@@ -38,8 +38,6 @@
 #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
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
index cd31ad2b8a7b..2fb80b6eb51d 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -98,7 +98,7 @@ static struct ieee80211_channel rtw_2ghz_channels[] = {
 
 static void rtw_2g_channels_init(struct ieee80211_channel *channels)
 {
-	memcpy((void*)channels, (void*)rtw_2ghz_channels,
+	memcpy((void *)channels, (void *)rtw_2ghz_channels,
 		sizeof(struct ieee80211_channel)*RTW_2G_CHANNELS_NUM
 	);
 }
@@ -133,8 +133,8 @@ static struct ieee80211_supported_band *rtw_spt_band_alloc(
 	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->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;
@@ -152,22 +152,6 @@ 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(spt_band);
-}
-
 static const struct ieee80211_txrx_stypes
 rtw_cfg80211_default_mgmt_stypes[NUM_NL80211_IFTYPES] = {
 	[NL80211_IFTYPE_ADHOC] = {
@@ -358,7 +342,7 @@ struct cfg80211_bss *rtw_cfg80211_inform_bss(struct adapter *padapter, struct wl
 	memcpy(pbuf, pnetwork->network.IEs, pnetwork->network.IELength);
 	len += pnetwork->network.IELength;
 
-	*((__le64*)pbuf) = cpu_to_le64(notify_timestamp);
+	*((__le64 *)pbuf) = cpu_to_le64(notify_timestamp);
 
 	bss = cfg80211_inform_bss_frame(wiphy, notify_channel, (struct ieee80211_mgmt *)buf,
 		len, notify_signal, GFP_ATOMIC);
@@ -1129,7 +1113,7 @@ static int cfg80211_rtw_add_key(struct wiphy *wiphy, struct net_device *ndev,
 	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)
 	{
 		if (mac_addr)
-			memcpy(param->sta_addr, (void*)mac_addr, ETH_ALEN);
+			memcpy(param->sta_addr, (void *)mac_addr, ETH_ALEN);
 
 		ret = rtw_cfg80211_ap_set_encryption(ndev, param, param_len);
 	}
@@ -2485,7 +2469,7 @@ static netdev_tx_t rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb, struc
 		 * for two MAC addresses
 		 */
 		skb_pull(skb, dot11_hdr_len + qos_len + snap_len - sizeof(src_mac_addr) * 2);
-		pdata = (unsigned char*)skb->data;
+		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));
 
@@ -2540,7 +2524,7 @@ static netdev_tx_t rtw_cfg80211_monitor_if_xmit_entry(struct sk_buff *skb, struc
 
 		pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 
-		memcpy(pframe, (void*)buf, len);
+		memcpy(pframe, (void *)buf, len);
 		pattrib->pktlen = len;
 
 		pwlanhdr = (struct ieee80211_hdr *)pframe;
@@ -3030,7 +3014,7 @@ static int _cfg80211_rtw_mgmt_tx(struct adapter *padapter, u8 tx_ch, const u8 *b
 
 	pframe = (u8 *)(pmgntframe->buf_addr) + TXDESC_OFFSET;
 
-	memcpy(pframe, (void*)buf, len);
+	memcpy(pframe, (void *)buf, len);
 	pattrib->pktlen = len;
 
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
@@ -3463,7 +3447,7 @@ void rtw_wdev_free(struct wireless_dev *wdev)
 	if (!wdev)
 		return;
 
-	rtw_spt_band_free(wdev->wiphy->bands[NL80211_BAND_2GHZ]);
+	kfree(wdev->wiphy->bands[NL80211_BAND_2GHZ]);
 
 	wiphy_free(wdev->wiphy);
 
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
index 5059b874080e..902ac8169948 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_linux.c
@@ -2561,14 +2561,16 @@ static int rtw_wps_start(struct net_device *dev,
 	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 (copy_from_user((void *)&u32wps_start, pdata->pointer, 4)) {
+		ret = -EFAULT;
+		goto exit;
+	}
 	if (u32wps_start == 0)
 		u32wps_start = *extra;
 
diff --git a/drivers/staging/rtl8723bs/os_dep/os_intfs.c b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
index d29f59bbb613..50a3c2c3a8d2 100644
--- a/drivers/staging/rtl8723bs/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8723bs/os_dep/os_intfs.c
@@ -1057,9 +1057,9 @@ static int pm_netdev_open(struct net_device *pnetdev, u8 bnormal)
 			status = _netdev_open(pnetdev);
 			mutex_unlock(&(adapter_to_dvobj(padapter)->hw_init_mutex));
 		}
-	}
-	else
+	} else {
 		status =  (_SUCCESS == ips_netdrv_open(padapter)) ? (0) : (-1);
+	}
 
 	return status;
 }
@@ -1192,8 +1192,7 @@ void rtw_dev_unload(struct adapter *padapter)
 		padapter->bup = false;
 
 		DBG_871X("<=== %s\n", __func__);
-	}
-	else {
+	} else {
 		RT_TRACE(_module_hci_intfs_c_, _drv_notice_, ("%s: bup ==false\n", __func__));
 		DBG_871X("%s: bup ==false\n", __func__);
 	}
@@ -1223,8 +1222,7 @@ static int rtw_suspend_free_assoc_resource(struct adapter *padapter)
 		rtw_disassoc_cmd(padapter, 0, false);
 		/* s2-2.  indicate disconnect to os */
 		rtw_indicate_disconnect(padapter);
-	}
-	else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
+	} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
 		rtw_sta_flush(padapter);
 	}
 
@@ -1270,9 +1268,8 @@ void rtw_suspend_wow(struct adapter *padapter)
 		padapter->bDriverStopped = false;	/* for 32k command */
 
 		/*  2. disable interrupt */
-		if (padapter->intf_stop) {
+		if (padapter->intf_stop)
 			padapter->intf_stop(padapter);
-		}
 
 		/*  2.1 clean interrupt */
 		if (padapter->HalFunc.clear_interrupt)
@@ -1448,14 +1445,13 @@ int rtw_suspend_common(struct adapter *padapter)
 
 	if (check_fwstate(pmlmepriv, WIFI_STATION_STATE) == true) {
 	#ifdef CONFIG_WOWLAN
-		if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+		if (check_fwstate(pmlmepriv, _FW_LINKED))
 			pwrpriv->wowlan_mode = true;
-		} else if (pwrpriv->wowlan_pno_enable == true) {
+		else if (pwrpriv->wowlan_pno_enable == true)
 			pwrpriv->wowlan_mode |= pwrpriv->wowlan_pno_enable;
-		}
 
 		if (pwrpriv->wowlan_mode == true)
-		rtw_suspend_wow(padapter);
+			rtw_suspend_wow(padapter);
 		else
 			rtw_suspend_normal(padapter);
 
@@ -1522,9 +1518,8 @@ int rtw_resume_process_wow(struct adapter *padapter)
 
 		pwrpriv->bFwCurrentInPSMode = false;
 
-		if (padapter->intf_stop) {
+		if (padapter->intf_stop)
 			padapter->intf_stop(padapter);
-		}
 
 		if (padapter->HalFunc.clear_interrupt)
 			padapter->HalFunc.clear_interrupt(padapter);
@@ -1541,18 +1536,15 @@ int rtw_resume_process_wow(struct adapter *padapter)
 		padapter->HalFunc.SetHwRegHandler(padapter, HW_VAR_WOWLAN, (u8 *)&poidparam);
 
 		psta = rtw_get_stainfo(&padapter->stapriv, get_bssid(&padapter->mlmepriv));
-		if (psta) {
+		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) {
+		if (padapter->intf_start)
 			padapter->intf_start(padapter);
-		}
 
 		/*  start netif queue */
 		if (pnetdev) {
@@ -1656,9 +1648,8 @@ int rtw_resume_process_ap_wow(struct adapter *padapter)
 	DBG_871X("%s: wowmode resuming, DriverStopped:%d\n", __func__, padapter->bDriverStopped);
 	rtw_start_drv_threads(padapter);
 
-	if (padapter->intf_start) {
+	if (padapter->intf_start)
 		padapter->intf_start(padapter);
-	}
 
 	/*  start netif queue */
 	if (pnetdev) {
diff --git a/drivers/staging/rtl8723bs/os_dep/recv_linux.c b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
index 60c35d92ba29..eb4d1c3008fe 100644
--- a/drivers/staging/rtl8723bs/os_dep/recv_linux.c
+++ b/drivers/staging/rtl8723bs/os_dep/recv_linux.c
@@ -9,6 +9,7 @@
 #include <drv_types.h>
 #include <rtw_debug.h>
 #include <linux/jiffies.h>
+#include <net/cfg80211.h>
 
 void rtw_os_free_recvframe(union recv_frame *precvframe)
 {
@@ -60,27 +61,20 @@ _pkt *rtw_os_alloc_msdu_pkt(union recv_frame *prframe, u16 nSubframe_Length, u8
 	pattrib = &prframe->u.hdr.attrib;
 
 	sub_skb = rtw_skb_alloc(nSubframe_Length + 12);
-	if (sub_skb) {
-		skb_reserve(sub_skb, 12);
-		skb_put_data(sub_skb, (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;
-		}
+	if (!sub_skb) {
+		DBG_871X("%s(): rtw_skb_alloc() Fail!!!\n", __func__);
+		return NULL;
 	}
 
+	skb_reserve(sub_skb, 12);
+	skb_put_data(sub_skb, (pdata + ETH_HLEN), nSubframe_Length);
+
 	eth_type = RTW_GET_BE16(&sub_skb->data[6]);
 
 	if (sub_skb->len >= 8 &&
-		((!memcmp(sub_skb->data, rtw_rfc1042_header, SNAP_SIZE) &&
+		((!memcmp(sub_skb->data, rfc1042_header, SNAP_SIZE) &&
 		  eth_type != ETH_P_AARP && eth_type != ETH_P_IPX) ||
-		 !memcmp(sub_skb->data, rtw_bridge_tunnel_header, SNAP_SIZE))) {
+		 !memcmp(sub_skb->data, bridge_tunnel_header, SNAP_SIZE))) {
 		/*
 		 * remove RFC1042 or Bridge-Tunnel encapsulation and replace
 		 * EtherType
@@ -230,7 +224,7 @@ static void rtw_os_ksocket_send(struct adapter *padapter, union recv_frame *prec
 
 		if (rx_pid == psta->pid) {
 			int i;
-			u16 len = *(u16*)(skb->data+ETH_HLEN+2);
+			u16 len = *(u16 *)(skb->data+ETH_HLEN+2);
 			DBG_871X("eth, RC: len = 0x%x\n", len);
 
 			for (i = 0; i < len; i++)
diff --git a/drivers/staging/rtl8723bs/os_dep/sdio_intf.c b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
index b093b5629171..5b1392deb0a7 100644
--- a/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
@@ -514,7 +514,7 @@ static void rtw_dev_remove(struct sdio_func *func)
 
 	rtw_unregister_netdevs(dvobj);
 
-	if (padapter->bSurpriseRemoved == false) {
+	if (!padapter->bSurpriseRemoved) {
 		int err;
 
 		/* test surprise remove */
@@ -554,12 +554,12 @@ static int rtw_sdio_suspend(struct device *dev)
 	struct adapter *padapter = psdpriv->if1;
 	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
 
-	if (padapter->bDriverStopped == true) {
+	if (padapter->bDriverStopped) {
 		DBG_871X("%s bDriverStopped = %d\n", __func__, padapter->bDriverStopped);
 		return 0;
 	}
 
-	if (pwrpriv->bInSuspend == true) {
+	if (pwrpriv->bInSuspend) {
 		DBG_871X("%s bInSuspend = %d\n", __func__, pwrpriv->bInSuspend);
 		pdbgpriv->dbg_suspend_error_cnt++;
 		return 0;
@@ -574,7 +574,7 @@ static int rtw_resume_process(struct adapter *padapter)
 	struct dvobj_priv *psdpriv = padapter->dvobj;
 	struct debug_priv *pdbgpriv = &psdpriv->drv_dbg;
 
-	if (pwrpriv->bInSuspend == false) {
+	if (!pwrpriv->bInSuspend) {
 		pdbgpriv->dbg_resume_error_cnt++;
 		DBG_871X("%s bInSuspend = %d\n", __func__, pwrpriv->bInSuspend);
 		return -1;
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index 59568d18ce23..a1a82e59dfee 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -297,6 +297,62 @@ static int lynxfb_ops_pan_display(struct fb_var_screeninfo *var,
 	return hw_sm750_pan_display(crtc, var, info);
 }
 
+static inline void lynxfb_set_visual_mode(struct fb_info *info)
+{
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
+		break;
+	case 16:
+	case 24:
+	case 32:
+		info->fix.visual = FB_VISUAL_TRUECOLOR;
+		break;
+	default:
+		break;
+	}
+}
+
+static inline int lynxfb_set_color_offsets(struct fb_info *info)
+{
+	lynxfb_set_visual_mode(info);
+
+	switch (info->var.bits_per_pixel) {
+	case 8:
+		info->var.red.offset = 0;
+		info->var.red.length = 8;
+		info->var.green.offset = 0;
+		info->var.green.length = 8;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 8;
+		info->var.transp.length = 0;
+		info->var.transp.offset = 0;
+		break;
+	case 16:
+		info->var.red.offset = 11;
+		info->var.red.length = 5;
+		info->var.green.offset = 5;
+		info->var.green.length = 6;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 5;
+		info->var.transp.length = 0;
+		info->var.transp.offset = 0;
+		break;
+	case 24:
+	case 32:
+		info->var.red.offset = 16;
+		info->var.red.length = 8;
+		info->var.green.offset = 8;
+		info->var.green.length = 8;
+		info->var.blue.offset = 0;
+		info->var.blue.length = 8;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
 static int lynxfb_ops_set_par(struct fb_info *info)
 {
 	struct lynxfb_par *par;
@@ -328,48 +384,13 @@ static int lynxfb_ops_set_par(struct fb_info *info)
 	 * and these data should be set before setcolreg routine
 	 */
 
-	switch (var->bits_per_pixel) {
-	case 8:
-		fix->visual = FB_VISUAL_PSEUDOCOLOR;
-		var->red.offset = 0;
-		var->red.length = 8;
-		var->green.offset = 0;
-		var->green.length = 8;
-		var->blue.offset = 0;
-		var->blue.length = 8;
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		break;
-	case 16:
-		var->red.offset = 11;
-		var->red.length = 5;
-		var->green.offset = 5;
-		var->green.length = 6;
-		var->blue.offset = 0;
-		var->blue.length = 5;
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		fix->visual = FB_VISUAL_TRUECOLOR;
-		break;
-	case 24:
-	case 32:
-		var->red.offset = 16;
-		var->red.length = 8;
-		var->green.offset = 8;
-		var->green.length = 8;
-		var->blue.offset = 0;
-		var->blue.length = 8;
-		fix->visual = FB_VISUAL_TRUECOLOR;
-		break;
-	default:
-		ret = -EINVAL;
-		break;
-	}
+	ret = lynxfb_set_color_offsets(info);
+
 	var->height = var->width = -1;
 	var->accel_flags = 0;/*FB_ACCELF_TEXT;*/
 
 	if (ret) {
-		pr_err("pixel bpp format not satisfied\n.");
+		pr_err("bpp %d not supported\n", var->bits_per_pixel);
 		return ret;
 	}
 	ret = hw_sm750_crtc_setMode(crtc, var, fix);
@@ -511,10 +532,12 @@ lynxfb_resume_err:
 static int lynxfb_ops_check_var(struct fb_var_screeninfo *var,
 				struct fb_info *info)
 {
+	int ret;
 	struct lynxfb_par *par;
 	struct lynxfb_crtc *crtc;
 	resource_size_t request;
 
+	ret = 0;
 	par = info->par;
 	crtc = &par->crtc;
 
@@ -523,43 +546,13 @@ static int lynxfb_ops_check_var(struct fb_var_screeninfo *var,
 		 var->yres,
 		 var->bits_per_pixel);
 
-	switch (var->bits_per_pixel) {
-	case 8:
-		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
-		var->red.offset = 0;
-		var->red.length = 8;
-		var->green.offset = 0;
-		var->green.length = 8;
-		var->blue.offset = 0;
-		var->blue.length = 8;
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		break;
-	case 16:
-		var->red.offset = 11;
-		var->red.length = 5;
-		var->green.offset = 5;
-		var->green.length = 6;
-		var->blue.offset = 0;
-		var->blue.length = 5;
-		var->transp.length = 0;
-		var->transp.offset = 0;
-		info->fix.visual = FB_VISUAL_TRUECOLOR;
-		break;
-	case 24:
-	case 32:
-		var->red.offset = 16;
-		var->red.length = 8;
-		var->green.offset = 8;
-		var->green.length = 8;
-		var->blue.offset = 0;
-		var->blue.length = 8;
-		info->fix.visual = FB_VISUAL_TRUECOLOR;
-		break;
-	default:
+	ret = lynxfb_set_color_offsets(info);
+
+	if (ret) {
 		pr_err("bpp %d not supported\n", var->bits_per_pixel);
-		return -EINVAL;
+		return ret;
 	}
+
 	var->height = var->width = -1;
 	var->accel_flags = 0;/* FB_ACCELF_TEXT; */
 
@@ -709,7 +702,9 @@ static int sm750fb_set_drv(struct lynxfb_par *par)
 		} else {
 			output->paths = sm750_crt;
 			crtc->channel = sm750_primary;
-			/* not consider of padding stuffs for oScreen,need fix */
+			/* not consider of padding stuffs for oScreen,
+			 * need fix
+			 */
 			crtc->oScreen = sm750_dev->vidmem_size >> 1;
 			crtc->vScreen = sm750_dev->pvMem + crtc->oScreen;
 		}
@@ -893,15 +888,8 @@ static int lynxfb_set_fbinfo(struct fb_info *info, int index)
 	pr_info("fix->mmio_start = %lx\n", fix->mmio_start);
 	fix->mmio_len = sm750_dev->vidreg_size;
 	pr_info("fix->mmio_len = %x\n", fix->mmio_len);
-	switch (var->bits_per_pixel) {
-	case 8:
-		fix->visual = FB_VISUAL_PSEUDOCOLOR;
-		break;
-	case 16:
-	case 32:
-		fix->visual = FB_VISUAL_TRUECOLOR;
-		break;
-	}
+
+	lynxfb_set_visual_mode(info);
 
 	/* set var */
 	var->activate = FB_ACTIVATE_NOW;
diff --git a/drivers/staging/sm750fb/sm750.h b/drivers/staging/sm750fb/sm750.h
index ce90adcb449d..19823c7277a4 100644
--- a/drivers/staging/sm750fb/sm750.h
+++ b/drivers/staging/sm750fb/sm750.h
@@ -59,16 +59,19 @@ struct lynx_accel {
 
 	int (*de_wait)(void);/* see if hardware ready to work */
 
-	int (*de_fillrect)(struct lynx_accel *, u32, u32, u32, u32,
-						u32, u32, u32, u32, u32);
+	int (*de_fillrect)(struct lynx_accel *,
+			   u32, u32, u32, u32,
+			   u32, u32, u32, u32, u32);
 
-	int (*de_copyarea)(struct lynx_accel *, u32, u32, u32, u32,
-						u32, u32, u32, u32,
-						u32, u32, u32, u32);
+	int (*de_copyarea)(struct lynx_accel *,
+			   u32, u32, u32, u32,
+			   u32, u32, u32, u32,
+			   u32, u32, u32, u32);
 
-	int (*de_imageblit)(struct lynx_accel *, const char *, u32, u32, u32, u32,
-							       u32, u32, u32, u32,
-							       u32, u32, u32, u32);
+	int (*de_imageblit)(struct lynx_accel *, const char *,
+			    u32, u32, u32, u32,
+			    u32, u32, u32, u32,
+			    u32, u32, u32, u32);
 
 };
 
@@ -163,7 +166,7 @@ struct lynxfb_output {
 	 */
 	void *priv;
 
-	int (*proc_setBLANK)(struct lynxfb_output*, int);
+	int (*proc_setBLANK)(struct lynxfb_output *output, int blank);
 };
 
 struct lynxfb_par {
diff --git a/drivers/staging/sm750fb/sm750_hw.c b/drivers/staging/sm750fb/sm750_hw.c
index b8d60701f898..7136d751cff5 100644
--- a/drivers/staging/sm750fb/sm750_hw.c
+++ b/drivers/staging/sm750fb/sm750_hw.c
@@ -51,7 +51,7 @@ int hw_sm750_map(struct sm750_dev *sm750_dev, struct pci_dev *pdev)
 
 	/* now map mmio and vidmem */
 	sm750_dev->pvReg = ioremap(sm750_dev->vidreg_start,
-					   sm750_dev->vidreg_size);
+				   sm750_dev->vidreg_size);
 	if (!sm750_dev->pvReg) {
 		pr_err("mmio failed\n");
 		ret = -EFAULT;
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index ddbb7e97d118..7408eb29cf38 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -43,6 +43,7 @@ static struct var_t vars[] = {
 	{ CAPS_STOP, .u.s = {"[:dv ap 100]" } },
 	{ RATE, .u.n = {"[:ra %d]", 7, 0, 9, 150, 25, NULL } },
 	{ PITCH, .u.n = {"[:dv ap %d]", 100, 0, 100, 0, 0, NULL } },
+	{ INFLECTION, .u.n = {"[:dv pr %d] ", 100, 0, 10000, 0, 0, NULL } },
 	{ VOL, .u.n = {"[:dv gv %d]", 13, 0, 16, 0, 5, NULL } },
 	{ PUNCT, .u.n = {"[:pu %c]", 0, 0, 2, 0, 0, "nsa" } },
 	{ VOICE, .u.n = {"[:n%c]", 0, 0, 9, 0, 0, "phfdburwkv" } },
@@ -59,6 +60,8 @@ static struct kobj_attribute caps_stop_attribute =
 	__ATTR(caps_stop, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
 	__ATTR(pitch, 0644, spk_var_show, spk_var_store);
+static struct kobj_attribute inflection_attribute =
+	__ATTR(inflection, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
 	__ATTR(punct, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
@@ -87,6 +90,7 @@ static struct attribute *synth_attrs[] = {
 	&caps_start_attribute.attr,
 	&caps_stop_attribute.attr,
 	&pitch_attribute.attr,
+	&inflection_attribute.attr,
 	&punct_attribute.attr,
 	&rate_attribute.attr,
 	&voice_attribute.attr,
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index 798c42dfa16c..96f24c848cc5 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -139,6 +139,7 @@ static struct var_t vars[] = {
 	{ CAPS_STOP, .u.s = {"[:dv ap 100]" } },
 	{ RATE, .u.n = {"[:ra %d]", 9, 0, 18, 150, 25, NULL } },
 	{ PITCH, .u.n = {"[:dv ap %d]", 80, 0, 100, 20, 0, NULL } },
+	{ INFLECTION, .u.n = {"[:dv pr %d] ", 100, 0, 10000, 0, 0, NULL } },
 	{ VOL, .u.n = {"[:vo se %d]", 5, 0, 9, 5, 10, NULL } },
 	{ PUNCT, .u.n = {"[:pu %c]", 0, 0, 2, 0, 0, "nsa" } },
 	{ VOICE, .u.n = {"[:n%c]", 0, 0, 9, 0, 0, "phfdburwkv" } },
@@ -155,6 +156,8 @@ static struct kobj_attribute caps_stop_attribute =
 	__ATTR(caps_stop, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
 	__ATTR(pitch, 0644, spk_var_show, spk_var_store);
+static struct kobj_attribute inflection_attribute =
+	__ATTR(inflection, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
 	__ATTR(punct, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
@@ -183,6 +186,7 @@ static struct attribute *synth_attrs[] = {
 	&caps_start_attribute.attr,
 	&caps_stop_attribute.attr,
 	&pitch_attribute.attr,
+	&inflection_attribute.attr,
 	&punct_attribute.attr,
 	&rate_attribute.attr,
 	&voice_attribute.attr,
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index dccb4ea29d37..780214b5ca16 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -44,7 +44,7 @@ static struct var_t vars[] = {
 	{ CAPS_START, .u.s = {"[:dv ap 160] " } },
 	{ CAPS_STOP, .u.s = {"[:dv ap 100 ] " } },
 	{ RATE, .u.n = {"[:ra %d] ", 180, 75, 650, 0, 0, NULL } },
-	{ PITCH, .u.n = {"[:dv ap %d] ", 122, 50, 350, 0, 0, NULL } },
+	{ INFLECTION, .u.n = {"[:dv pr %d] ", 100, 0, 10000, 0, 0, NULL } },
 	{ VOL, .u.n = {"[:dv g5 %d] ", 86, 60, 86, 0, 0, NULL } },
 	{ PUNCT, .u.n = {"[:pu %c] ", 0, 0, 2, 0, 0, "nsa" } },
 	{ VOICE, .u.n = {"[:n%c] ", 0, 0, 9, 0, 0, "phfdburwkv" } },
@@ -61,6 +61,8 @@ static struct kobj_attribute caps_stop_attribute =
 	__ATTR(caps_stop, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
 	__ATTR(pitch, 0644, spk_var_show, spk_var_store);
+static struct kobj_attribute inflection_attribute =
+	__ATTR(inflection, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
 	__ATTR(punct, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
@@ -89,6 +91,7 @@ static struct attribute *synth_attrs[] = {
 	&caps_start_attribute.attr,
 	&caps_stop_attribute.attr,
 	&pitch_attribute.attr,
+	&inflection_attribute.attr,
 	&punct_attribute.attr,
 	&rate_attribute.attr,
 	&voice_attribute.attr,
diff --git a/drivers/staging/speakup/speakup_dummy.c b/drivers/staging/speakup/speakup_dummy.c
index 7df1a84297f6..e393438af81b 100644
--- a/drivers/staging/speakup/speakup_dummy.c
+++ b/drivers/staging/speakup/speakup_dummy.c
@@ -24,6 +24,7 @@ static struct var_t vars[] = {
 	{ PAUSE, .u.s = {"PAUSE\n"} },
 	{ RATE, .u.n = {"RATE %d\n", 8, 1, 16, 0, 0, NULL } },
 	{ PITCH, .u.n = {"PITCH %d\n", 8, 0, 16, 0, 0, NULL } },
+	{ INFLECTION, .u.n = {"INFLECTION %d\n", 8, 0, 16, 0, 0, NULL } },
 	{ VOL, .u.n = {"VOL %d\n", 8, 0, 16, 0, 0, NULL } },
 	{ TONE, .u.n = {"TONE %d\n", 8, 0, 16, 0, 0, NULL } },
 	{ DIRECT, .u.n = {NULL, 0, 0, 1, 0, 0, NULL } },
@@ -39,6 +40,8 @@ static struct kobj_attribute caps_stop_attribute =
 	__ATTR(caps_stop, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
 	__ATTR(pitch, 0644, spk_var_show, spk_var_store);
+static struct kobj_attribute inflection_attribute =
+	__ATTR(inflection, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
 	__ATTR(rate, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute tone_attribute =
@@ -65,6 +68,7 @@ static struct attribute *synth_attrs[] = {
 	&caps_start_attribute.attr,
 	&caps_stop_attribute.attr,
 	&pitch_attribute.attr,
+	&inflection_attribute.attr,
 	&rate_attribute.attr,
 	&tone_attribute.attr,
 	&vol_attribute.attr,
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index f591ec095582..9a7029539f35 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -38,6 +38,7 @@ static struct var_t vars[] = {
 	{ PAUSE, .u.n = {"\x01P" } },
 	{ RATE, .u.n = {"\x01%ds", 2, 0, 9, 0, 0, NULL } },
 	{ PITCH, .u.n = {"\x01%dp", 5, 0, 9, 0, 0, NULL } },
+	{ INFLECTION, .u.n = {"\x01%dr", 5, 0, 9, 0, 0, NULL } },
 	{ VOL, .u.n = {"\x01%dv", 5, 0, 9, 0, 0, NULL } },
 	{ TONE, .u.n = {"\x01%dx", 1, 0, 2, 0, 0, NULL } },
 	{ PUNCT, .u.n = {"\x01%db", 0, 0, 2, 0, 0, NULL } },
@@ -57,6 +58,8 @@ static struct kobj_attribute freq_attribute =
 	__ATTR(freq, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute pitch_attribute =
 	__ATTR(pitch, 0644, spk_var_show, spk_var_store);
+static struct kobj_attribute inflection_attribute =
+	__ATTR(inflection, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute punct_attribute =
 	__ATTR(punct, 0644, spk_var_show, spk_var_store);
 static struct kobj_attribute rate_attribute =
@@ -96,6 +99,7 @@ static struct attribute *synth_attrs[] = {
 	&freq_attribute.attr,
 /*	&lang_attribute.attr, */
 	&pitch_attribute.attr,
+	&inflection_attribute.attr,
 	&punct_attribute.attr,
 	&rate_attribute.attr,
 	&tone_attribute.attr,
diff --git a/drivers/staging/speakup/spk_types.h b/drivers/staging/speakup/spk_types.h
index fc6a9416829c..d3272c6d199a 100644
--- a/drivers/staging/speakup/spk_types.h
+++ b/drivers/staging/speakup/spk_types.h
@@ -42,7 +42,8 @@ enum var_id_t {
 	SAY_CONTROL, SAY_WORD_CTL, NO_INTERRUPT, KEY_ECHO,
 	SPELL_DELAY, PUNC_LEVEL, READING_PUNC,
 	ATTRIB_BLEEP, BLEEPS,
-	RATE, PITCH, VOL, TONE, PUNCT, VOICE, FREQUENCY, LANG, DIRECT, PAUSE,
+	RATE, PITCH, INFLECTION, VOL, TONE, PUNCT, VOICE, FREQUENCY, LANG,
+	DIRECT, PAUSE,
 	CAPS_START, CAPS_STOP, CHARTAB,
 	MAXVARS
 };
diff --git a/drivers/staging/speakup/spkguide.txt b/drivers/staging/speakup/spkguide.txt
index c23549c54c3c..1e622cd34363 100644
--- a/drivers/staging/speakup/spkguide.txt
+++ b/drivers/staging/speakup/spkguide.txt
@@ -406,6 +406,7 @@ freq
 full_time
 jiffy_delta
 pitch
+inflection
 punct
 rate
 tone
@@ -518,9 +519,9 @@ All the entries in the Speakup sys system are readable, some are
 writable by root only, and some are writable by everyone.  Unless you
 know what you are doing, you should probably leave the ones that are
 writable by root only alone.  Most of the names are self explanatory.
-Vol for controlling volume, pitch for pitch, rate for controlling speaking
-rate, etc.  If you find one you aren't sure about, you can post a query
-on the Speakup list.
+Vol for controlling volume, pitch for pitch, inflection for pitch range, rate
+for controlling speaking rate, etc.  If you find one you aren't sure about, you
+can post a query on the Speakup list.
 
 6.  Changing Synthesizers
 
diff --git a/drivers/staging/speakup/sysfs-driver-speakup b/drivers/staging/speakup/sysfs-driver-speakup
index be3f5d6962e9..c6a32c434ce9 100644
--- a/drivers/staging/speakup/sysfs-driver-speakup
+++ b/drivers/staging/speakup/sysfs-driver-speakup
@@ -325,6 +325,12 @@ KernelVersion:	2.6
 Contact:	speakup@linux-speakup.org
 Description:	Gets or sets the pitch of the synthesizer. The range is 0-9.
 
+What:		/sys/accessibility/speakup/soft/inflection
+KernelVersion:	5.8
+Contact:	speakup@linux-speakup.org
+Description:	Gets or sets the inflection of the synthesizer, i.e. the pitch
+		range. The range is 0-9.
+
 What:		/sys/accessibility/speakup/soft/punct
 KernelVersion:	2.6
 Contact:	speakup@linux-speakup.org
diff --git a/drivers/staging/speakup/varhandlers.c b/drivers/staging/speakup/varhandlers.c
index 5741d1cb6227..d7f6bec7ff06 100644
--- a/drivers/staging/speakup/varhandlers.c
+++ b/drivers/staging/speakup/varhandlers.c
@@ -37,6 +37,7 @@ static struct st_var_header var_headers[] = {
 	{ "bell_pos", BELL_POS, VAR_NUM, &spk_bell_pos, NULL },
 	{ "rate", RATE, VAR_NUM, NULL, NULL },
 	{ "pitch", PITCH, VAR_NUM, NULL, NULL },
+	{ "inflection", INFLECTION, VAR_NUM, NULL, NULL },
 	{ "vol", VOL, VAR_NUM, NULL, NULL },
 	{ "tone", TONE, VAR_NUM, NULL, NULL },
 	{ "punct", PUNCT, VAR_NUM, NULL, NULL   },
diff --git a/drivers/staging/unisys/visorhba/visorhba_main.c b/drivers/staging/unisys/visorhba/visorhba_main.c
index dd979ee4dcf1..99c57ceeb357 100644
--- a/drivers/staging/unisys/visorhba/visorhba_main.c
+++ b/drivers/staging/unisys/visorhba/visorhba_main.c
@@ -292,7 +292,7 @@ static void cleanup_scsitaskmgmt_handles(struct idr *idrtable,
  * @tasktype: Type of taskmgmt command
  * @scsidev:  Scsidev that issued command
  *
- * Create a cmdrsp packet and send it to the Serivce Partition
+ * Create a cmdrsp packet and send it to the Service Partition
  * that will service this request.
  *
  * Return: Int representing whether command was queued successfully or not
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
index 33485184a98a..f783b632141b 100644
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
@@ -233,7 +233,7 @@ static int snd_bcm2835_pcm_prepare(struct snd_pcm_substream *substream)
 }
 
 static void snd_bcm2835_pcm_transfer(struct snd_pcm_substream *substream,
-	struct snd_pcm_indirect *rec, size_t bytes)
+				     struct snd_pcm_indirect *rec, size_t bytes)
 {
 	struct snd_pcm_runtime *runtime = substream->runtime;
 	struct bcm2835_alsa_stream *alsa_stream = runtime->private_data;
@@ -346,7 +346,7 @@ int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name,
 			&snd_bcm2835_playback_ops);
 
 	snd_pcm_set_managed_buffer_all(pcm, SNDRV_DMA_TYPE_DEV,
-		chip->card->dev, 128 * 1024, 128 * 1024);
+				       chip->card->dev, 128 * 1024, 128 * 1024);
 
 	if (spdif)
 		chip->pcm_spdif = pcm;
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
index 597acef35d0b..4f1adddb804f 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
@@ -369,8 +369,8 @@ static void buffer_cb(struct vchiq_mmal_instance *instance,
 
 	if (dev->capture.vc_start_timestamp != -1 && pts) {
 		ktime_t timestamp;
-		s64 runtime_us = pts -
-		    dev->capture.vc_start_timestamp;
+		s64 runtime_us = pts - dev->capture.vc_start_timestamp;
+
 		timestamp = ktime_add_us(dev->capture.kernel_start_ts,
 					 runtime_us);
 		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
@@ -420,9 +420,8 @@ static int enable_camera(struct bm2835_mmal_dev *dev)
 			return -EINVAL;
 		}
 
-		ret = vchiq_mmal_component_enable(
-				dev->instance,
-				dev->component[COMP_CAMERA]);
+		ret = vchiq_mmal_component_enable(dev->instance,
+						  dev->component[COMP_CAMERA]);
 		if (ret < 0) {
 			v4l2_err(&dev->v4l2_dev,
 				 "Failed enabling camera, ret %d\n", ret);
@@ -451,10 +450,8 @@ static int disable_camera(struct bm2835_mmal_dev *dev)
 
 		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
 			 "Disabling camera\n");
-		ret =
-		    vchiq_mmal_component_disable(
-				dev->instance,
-				dev->component[COMP_CAMERA]);
+		ret = vchiq_mmal_component_disable(dev->instance,
+						   dev->component[COMP_CAMERA]);
 		if (ret < 0) {
 			v4l2_err(&dev->v4l2_dev,
 				 "Failed disabling camera, ret %d\n", ret);
@@ -555,8 +552,8 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	/* enable the camera port */
 	dev->capture.port->cb_ctx = dev;
-	ret =
-	    vchiq_mmal_port_enable(dev->instance, dev->capture.port, buffer_cb);
+	ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port,
+				     buffer_cb);
 	if (ret) {
 		v4l2_err(&dev->v4l2_dev,
 			 "Failed to enable capture port - error %d. Disabling camera port again\n",
@@ -668,7 +665,7 @@ static int set_overlay_params(struct bm2835_mmal_dev *dev,
 			MMAL_DISPLAY_SET_ALPHA |
 			MMAL_DISPLAY_SET_DEST_RECT |
 			MMAL_DISPLAY_SET_FULLSCREEN,
-		.layer = PREVIEW_LAYER,
+		.layer = 2,
 		.alpha = dev->overlay.global_alpha,
 		.fullscreen = 0,
 		.dest_rect = {
@@ -767,16 +764,14 @@ static int vidioc_overlay(struct file *file, void *f, unsigned int on)
 	    (!on && !dev->component[COMP_PREVIEW]->enabled))
 		return 0;	/* already in requested state */
 
-	src =
-	    &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
+	src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
 
 	if (!on) {
 		/* disconnect preview ports and disable component */
 		ret = vchiq_mmal_port_disable(dev->instance, src);
 		if (!ret)
-			ret =
-			    vchiq_mmal_port_connect_tunnel(dev->instance, src,
-							   NULL);
+			ret = vchiq_mmal_port_connect_tunnel(dev->instance, src,
+							     NULL);
 		if (ret >= 0)
 			ret = vchiq_mmal_component_disable(
 					dev->instance,
@@ -800,9 +795,8 @@ static int vidioc_overlay(struct file *file, void *f, unsigned int on)
 	if (enable_camera(dev) < 0)
 		return -EINVAL;
 
-	ret = vchiq_mmal_component_enable(
-			dev->instance,
-			dev->component[COMP_PREVIEW]);
+	ret = vchiq_mmal_component_enable(dev->instance,
+					  dev->component[COMP_PREVIEW]);
 	if (ret < 0)
 		return ret;
 
@@ -1001,6 +995,141 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	return 0;
 }
 
+
+static int mmal_setup_video_component(struct bm2835_mmal_dev *dev,
+				      struct v4l2_format *f)
+{
+	bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled;
+	struct vchiq_mmal_port *preview_port;
+	int ret;
+
+	preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
+
+	/* Preview and encode ports need to match on resolution */
+	if (overlay_enabled) {
+		/* Need to disable the overlay before we can update
+		 * the resolution
+		 */
+		ret = vchiq_mmal_port_disable(dev->instance, preview_port);
+		if (!ret) {
+			ret = vchiq_mmal_port_connect_tunnel(dev->instance,
+							     preview_port,
+							     NULL);
+		}
+	}
+	preview_port->es.video.width = f->fmt.pix.width;
+	preview_port->es.video.height = f->fmt.pix.height;
+	preview_port->es.video.crop.x = 0;
+	preview_port->es.video.crop.y = 0;
+	preview_port->es.video.crop.width = f->fmt.pix.width;
+	preview_port->es.video.crop.height = f->fmt.pix.height;
+	preview_port->es.video.frame_rate.num =
+				  dev->capture.timeperframe.denominator;
+	preview_port->es.video.frame_rate.den =
+				  dev->capture.timeperframe.numerator;
+	ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
+
+	if (overlay_enabled) {
+		ret = vchiq_mmal_port_connect_tunnel(dev->instance,
+				preview_port,
+				&dev->component[COMP_PREVIEW]->input[0]);
+		if (ret)
+			return ret;
+
+		ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL);
+	}
+
+	return ret;
+}
+
+static int mmal_setup_encode_component(struct bm2835_mmal_dev *dev,
+				       struct v4l2_format *f,
+				       struct vchiq_mmal_port *port,
+				       struct vchiq_mmal_port *camera_port,
+				       struct vchiq_mmal_component *component)
+{
+	struct mmal_fmt *mfmt = get_format(f);
+	int ret;
+
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "vid_cap - set up encode comp\n");
+
+	/* configure buffering */
+	camera_port->current_buffer.size = camera_port->recommended_buffer.size;
+	camera_port->current_buffer.num = camera_port->recommended_buffer.num;
+
+	ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port,
+					     &component->input[0]);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s failed to create connection\n", __func__);
+		/* ensure capture is not going to be tried */
+		dev->capture.port = NULL;
+		return ret;
+	}
+
+	port->es.video.width = f->fmt.pix.width;
+	port->es.video.height = f->fmt.pix.height;
+	port->es.video.crop.x = 0;
+	port->es.video.crop.y = 0;
+	port->es.video.crop.width = f->fmt.pix.width;
+	port->es.video.crop.height = f->fmt.pix.height;
+	port->es.video.frame_rate.num =
+		  dev->capture.timeperframe.denominator;
+	port->es.video.frame_rate.den =
+		  dev->capture.timeperframe.numerator;
+
+	port->format.encoding = mfmt->mmal;
+	port->format.encoding_variant = 0;
+	/* Set any encoding specific parameters */
+	switch (mfmt->mmal_component) {
+	case COMP_VIDEO_ENCODE:
+		port->format.bitrate = dev->capture.encode_bitrate;
+		break;
+	case COMP_IMAGE_ENCODE:
+		/* Could set EXIF parameters here */
+		break;
+	default:
+		break;
+	}
+
+	ret = vchiq_mmal_port_set_format(dev->instance, port);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s failed to set format %dx%d fmt %08X\n",
+			 __func__,
+			 f->fmt.pix.width,
+			 f->fmt.pix.height,
+			 f->fmt.pix.pixelformat);
+		return ret;
+	}
+
+	ret = vchiq_mmal_component_enable(dev->instance, component);
+	if (ret) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "%s Failed to enable encode components\n", __func__);
+		return ret;
+	}
+
+	/* configure buffering */
+	port->current_buffer.num = 1;
+	port->current_buffer.size = f->fmt.pix.sizeimage;
+	if (port->format.encoding == MMAL_ENCODING_JPEG) {
+		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+			 "JPG - buf size now %d was %d\n",
+			 f->fmt.pix.sizeimage,
+			 port->current_buffer.size);
+		port->current_buffer.size =
+		    (f->fmt.pix.sizeimage < (100 << 10)) ?
+		    (100 << 10) : f->fmt.pix.sizeimage;
+	}
+	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
+		 "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage);
+	port->current_buffer.alignment = 0;
+
+	return 0;
+}
+
 static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 				 struct v4l2_format *f)
 {
@@ -1075,8 +1204,7 @@ static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 	}
 
 	remove_padding = mfmt->remove_padding;
-	vchiq_mmal_port_parameter_set(dev->instance,
-				      camera_port,
+	vchiq_mmal_port_parameter_set(dev->instance, camera_port,
 				      MMAL_PARAMETER_NO_IMAGE_PADDING,
 				      &remove_padding, sizeof(remove_padding));
 
@@ -1096,46 +1224,7 @@ static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 	if (!ret &&
 	    camera_port ==
 	    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
-		bool overlay_enabled =
-		    !!dev->component[COMP_PREVIEW]->enabled;
-		struct vchiq_mmal_port *preview_port =
-		    &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
-		/* Preview and encode ports need to match on resolution */
-		if (overlay_enabled) {
-			/* Need to disable the overlay before we can update
-			 * the resolution
-			 */
-			ret =
-			    vchiq_mmal_port_disable(dev->instance,
-						    preview_port);
-			if (!ret)
-				ret =
-				    vchiq_mmal_port_connect_tunnel(
-						dev->instance,
-						preview_port,
-						NULL);
-		}
-		preview_port->es.video.width = f->fmt.pix.width;
-		preview_port->es.video.height = f->fmt.pix.height;
-		preview_port->es.video.crop.x = 0;
-		preview_port->es.video.crop.y = 0;
-		preview_port->es.video.crop.width = f->fmt.pix.width;
-		preview_port->es.video.crop.height = f->fmt.pix.height;
-		preview_port->es.video.frame_rate.num =
-					  dev->capture.timeperframe.denominator;
-		preview_port->es.video.frame_rate.den =
-					  dev->capture.timeperframe.numerator;
-		ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
-		if (overlay_enabled) {
-			ret = vchiq_mmal_port_connect_tunnel(
-				dev->instance,
-				preview_port,
-				&dev->component[COMP_PREVIEW]->input[0]);
-			if (!ret)
-				ret = vchiq_mmal_port_enable(dev->instance,
-							     preview_port,
-							     NULL);
-		}
+		ret = mmal_setup_video_component(dev, f);
 	}
 
 	if (ret) {
@@ -1145,128 +1234,39 @@ static int mmal_setup_components(struct bm2835_mmal_dev *dev,
 			 f->fmt.pix.pixelformat);
 		/* ensure capture is not going to be tried */
 		dev->capture.port = NULL;
-	} else {
-		if (encode_component) {
-			v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-				 "vid_cap - set up encode comp\n");
+		return ret;
+	}
 
-			/* configure buffering */
-			camera_port->current_buffer.size =
-			    camera_port->recommended_buffer.size;
-			camera_port->current_buffer.num =
-			    camera_port->recommended_buffer.num;
+	if (encode_component) {
+		ret = mmal_setup_encode_component(dev, f, port,
+						  camera_port,
+						  encode_component);
 
-			ret =
-			    vchiq_mmal_port_connect_tunnel(
-					dev->instance,
-					camera_port,
-					&encode_component->input[0]);
-			if (ret) {
-				v4l2_dbg(1, bcm2835_v4l2_debug,
-					 &dev->v4l2_dev,
-					 "%s failed to create connection\n",
-					 __func__);
-				/* ensure capture is not going to be tried */
-				dev->capture.port = NULL;
-			} else {
-				port->es.video.width = f->fmt.pix.width;
-				port->es.video.height = f->fmt.pix.height;
-				port->es.video.crop.x = 0;
-				port->es.video.crop.y = 0;
-				port->es.video.crop.width = f->fmt.pix.width;
-				port->es.video.crop.height = f->fmt.pix.height;
-				port->es.video.frame_rate.num =
-					  dev->capture.timeperframe.denominator;
-				port->es.video.frame_rate.den =
-					  dev->capture.timeperframe.numerator;
-
-				port->format.encoding = mfmt->mmal;
-				port->format.encoding_variant = 0;
-				/* Set any encoding specific parameters */
-				switch (mfmt->mmal_component) {
-				case COMP_VIDEO_ENCODE:
-					port->format.bitrate =
-					    dev->capture.encode_bitrate;
-					break;
-				case COMP_IMAGE_ENCODE:
-					/* Could set EXIF parameters here */
-					break;
-				default:
-					break;
-				}
-				ret = vchiq_mmal_port_set_format(dev->instance,
-								 port);
-				if (ret)
-					v4l2_dbg(1, bcm2835_v4l2_debug,
-						 &dev->v4l2_dev,
-						 "%s failed to set format %dx%d fmt %08X\n",
-						 __func__,
-						 f->fmt.pix.width,
-						 f->fmt.pix.height,
-						 f->fmt.pix.pixelformat
-						 );
-			}
+		if (ret)
+			return ret;
+	} else {
+		/* configure buffering */
+		camera_port->current_buffer.num = 1;
+		camera_port->current_buffer.size = f->fmt.pix.sizeimage;
+		camera_port->current_buffer.alignment = 0;
+	}
 
-			if (!ret) {
-				ret = vchiq_mmal_component_enable(
-						dev->instance,
-						encode_component);
-				if (ret) {
-					v4l2_dbg(1, bcm2835_v4l2_debug,
-						 &dev->v4l2_dev,
-						 "%s Failed to enable encode components\n",
-						 __func__);
-				}
-			}
-			if (!ret) {
-				/* configure buffering */
-				port->current_buffer.num = 1;
-				port->current_buffer.size =
-				    f->fmt.pix.sizeimage;
-				if (port->format.encoding ==
-				    MMAL_ENCODING_JPEG) {
-					v4l2_dbg(1, bcm2835_v4l2_debug,
-						 &dev->v4l2_dev,
-						 "JPG - buf size now %d was %d\n",
-						 f->fmt.pix.sizeimage,
-						 port->current_buffer.size);
-					port->current_buffer.size =
-					    (f->fmt.pix.sizeimage <
-					     (100 << 10)) ?
-					    (100 << 10) : f->fmt.pix.sizeimage;
-				}
-				v4l2_dbg(1, bcm2835_v4l2_debug,
-					 &dev->v4l2_dev,
-					 "vid_cap - cur_buf.size set to %d\n",
-					 f->fmt.pix.sizeimage);
-				port->current_buffer.alignment = 0;
-			}
-		} else {
-			/* configure buffering */
-			camera_port->current_buffer.num = 1;
-			camera_port->current_buffer.size = f->fmt.pix.sizeimage;
-			camera_port->current_buffer.alignment = 0;
-		}
+	dev->capture.fmt = mfmt;
+	dev->capture.stride = f->fmt.pix.bytesperline;
+	dev->capture.width = camera_port->es.video.crop.width;
+	dev->capture.height = camera_port->es.video.crop.height;
+	dev->capture.buffersize = port->current_buffer.size;
 
-		if (!ret) {
-			dev->capture.fmt = mfmt;
-			dev->capture.stride = f->fmt.pix.bytesperline;
-			dev->capture.width = camera_port->es.video.crop.width;
-			dev->capture.height = camera_port->es.video.crop.height;
-			dev->capture.buffersize = port->current_buffer.size;
-
-			/* select port for capture */
-			dev->capture.port = port;
-			dev->capture.camera_port = camera_port;
-			dev->capture.encode_component = encode_component;
-			v4l2_dbg(1, bcm2835_v4l2_debug,
-				 &dev->v4l2_dev,
-				"Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
-				port->format.encoding,
-				dev->capture.width, dev->capture.height,
-				dev->capture.stride, dev->capture.buffersize);
-		}
-	}
+	/* select port for capture */
+	dev->capture.port = port;
+	dev->capture.camera_port = camera_port;
+	dev->capture.encode_component = encode_component;
+	v4l2_dbg(1, bcm2835_v4l2_debug,
+		 &dev->v4l2_dev,
+		"Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
+		port->format.encoding,
+		dev->capture.width, dev->capture.height,
+		dev->capture.stride, dev->capture.buffersize);
 
 	/* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
 	return ret;
@@ -1658,9 +1658,8 @@ static int mmal_init(struct bm2835_mmal_dev *dev)
 	dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
 
 	/* get the preview component ready */
-	ret = vchiq_mmal_component_init(
-			dev->instance, "ril.video_render",
-			&dev->component[COMP_PREVIEW]);
+	ret = vchiq_mmal_component_init(dev->instance, "ril.video_render",
+					&dev->component[COMP_PREVIEW]);
 	if (ret < 0)
 		goto unreg_camera;
 
@@ -1672,9 +1671,8 @@ static int mmal_init(struct bm2835_mmal_dev *dev)
 	}
 
 	/* get the image encoder component ready */
-	ret = vchiq_mmal_component_init(
-		dev->instance, "ril.image_encode",
-		&dev->component[COMP_IMAGE_ENCODE]);
+	ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode",
+					&dev->component[COMP_IMAGE_ENCODE]);
 	if (ret < 0)
 		goto unreg_preview;
 
@@ -1734,15 +1732,13 @@ static int mmal_init(struct bm2835_mmal_dev *dev)
 
 unreg_vid_encoder:
 	pr_err("Cleanup: Destroy video encoder\n");
-	vchiq_mmal_component_finalise(
-		dev->instance,
-		dev->component[COMP_VIDEO_ENCODE]);
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_VIDEO_ENCODE]);
 
 unreg_image_encoder:
 	pr_err("Cleanup: Destroy image encoder\n");
-	vchiq_mmal_component_finalise(
-		dev->instance,
-		dev->component[COMP_IMAGE_ENCODE]);
+	vchiq_mmal_component_finalise(dev->instance,
+				      dev->component[COMP_IMAGE_ENCODE]);
 
 unreg_preview:
 	pr_err("Cleanup: Destroy video render\n");
@@ -1775,8 +1771,7 @@ static int bm2835_mmal_init_device(struct bm2835_mmal_dev *dev,
 	/* video device needs to be able to access instance data */
 	video_set_drvdata(vfd, dev);
 
-	ret = video_register_device(vfd,
-				    VFL_TYPE_VIDEO,
+	ret = video_register_device(vfd, VFL_TYPE_VIDEO,
 				    video_nr[dev->camera_num]);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
index b5fce38de038..75524adff0f5 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
@@ -30,79 +30,77 @@ enum {
 	CAM_PORT_COUNT
 };
 
-#define PREVIEW_LAYER      2
-
 extern int bcm2835_v4l2_debug;
 
 struct bm2835_mmal_dev {
 	/* v4l2 devices */
-	struct v4l2_device     v4l2_dev;
-	struct video_device    vdev;
-	struct mutex           mutex;
+	struct v4l2_device v4l2_dev;
+	struct video_device vdev;
+	struct mutex mutex;
 
 	/* controls */
-	struct v4l2_ctrl_handler  ctrl_handler;
-	struct v4l2_ctrl          *ctrls[V4L2_CTRL_COUNT];
-	enum v4l2_scene_mode	  scene_mode;
-	struct mmal_colourfx      colourfx;
-	int                       hflip;
-	int                       vflip;
-	int			  red_gain;
-	int			  blue_gain;
+	struct v4l2_ctrl_handler ctrl_handler;
+	struct v4l2_ctrl *ctrls[V4L2_CTRL_COUNT];
+	enum v4l2_scene_mode scene_mode;
+	struct mmal_colourfx colourfx;
+	int hflip;
+	int vflip;
+	int red_gain;
+	int blue_gain;
 	enum mmal_parameter_exposuremode exposure_mode_user;
 	enum v4l2_exposure_auto_type exposure_mode_v4l2_user;
 	/* active exposure mode may differ if selected via a scene mode */
 	enum mmal_parameter_exposuremode exposure_mode_active;
 	enum mmal_parameter_exposuremeteringmode metering_mode;
-	unsigned int		  manual_shutter_speed;
-	bool			  exp_auto_priority;
+	unsigned int manual_shutter_speed;
+	bool exp_auto_priority;
 	bool manual_iso_enabled;
 	u32 iso;
 
 	/* allocated mmal instance and components */
-	struct vchiq_mmal_instance   *instance;
-	struct vchiq_mmal_component  *component[COMP_COUNT];
+	struct vchiq_mmal_instance *instance;
+	struct vchiq_mmal_component *component[COMP_COUNT];
 	int camera_use_count;
 
 	struct v4l2_window overlay;
 
 	struct {
-		unsigned int     width;  /* width */
-		unsigned int     height;  /* height */
-		unsigned int     stride;  /* stride */
-		unsigned int     buffersize; /* buffer size with padding */
-		struct mmal_fmt  *fmt;
+		unsigned int width;  /* width */
+		unsigned int height;  /* height */
+		unsigned int stride;  /* stride */
+		unsigned int buffersize; /* buffer size with padding */
+		struct mmal_fmt *fmt;
 		struct v4l2_fract timeperframe;
 
 		/* H264 encode bitrate */
-		int         encode_bitrate;
+		int encode_bitrate;
 		/* H264 bitrate mode. CBR/VBR */
-		int         encode_bitrate_mode;
+		int encode_bitrate_mode;
 		/* H264 profile */
 		enum v4l2_mpeg_video_h264_profile enc_profile;
 		/* H264 level */
 		enum v4l2_mpeg_video_h264_level enc_level;
 		/* JPEG Q-factor */
-		int         q_factor;
+		int q_factor;
 
-		struct vb2_queue	vb_vidq;
+		struct vb2_queue vb_vidq;
 
 		/* VC start timestamp for streaming */
-		s64         vc_start_timestamp;
+		s64 vc_start_timestamp;
 		/* Kernel start timestamp for streaming */
 		ktime_t kernel_start_ts;
 		/* Sequence number of last buffer */
-		u32		sequence;
+		u32 sequence;
 
-		struct vchiq_mmal_port  *port; /* port being used for capture */
+		struct vchiq_mmal_port *port; /* port being used for capture */
 		/* camera port being used for capture */
-		struct vchiq_mmal_port  *camera_port;
+		struct vchiq_mmal_port *camera_port;
 		/* component being used for encode */
 		struct vchiq_mmal_component *encode_component;
 		/* number of frames remaining which driver should capture */
-		unsigned int  frame_count;
+		unsigned int frame_count;
 		/* last frame completion */
-		struct completion  frame_cmplt;
+		struct completion frame_cmplt;
 
 	} capture;
 
diff --git a/drivers/staging/vc04_services/bcm2835-camera/controls.c b/drivers/staging/vc04_services/bcm2835-camera/controls.c
index 5137fcf203d6..b096a12387f7 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/controls.c
+++ b/drivers/staging/vc04_services/bcm2835-camera/controls.c
@@ -135,8 +135,8 @@ static const struct v4l2_to_mmal_effects_setting
 };
 
 struct v4l2_mmal_scene_config {
-	enum v4l2_scene_mode			v4l2_scene;
-	enum mmal_parameter_exposuremode	exposure_mode;
+	enum v4l2_scene_mode v4l2_scene;
+	enum mmal_parameter_exposuremode exposure_mode;
 	enum mmal_parameter_exposuremeteringmode metering_mode;
 };
 
@@ -377,11 +377,9 @@ static int ctrl_set_metering_mode(struct bm2835_mmal_dev *dev,
 		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT;
 		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) {
@@ -516,42 +514,41 @@ static int ctrl_set_image_effect(struct bm2835_mmal_dev *dev,
 	struct mmal_parameter_imagefx_parameters imagefx;
 
 	for (i = 0; i < ARRAY_SIZE(v4l2_to_mmal_effects_values); i++) {
-		if (ctrl->val == v4l2_to_mmal_effects_values[i].v4l2_effect) {
-			imagefx.effect =
-				v4l2_to_mmal_effects_values[i].mmal_effect;
-			imagefx.num_effect_params =
-				v4l2_to_mmal_effects_values[i].num_effect_params;
-
-			if (imagefx.num_effect_params > MMAL_MAX_IMAGEFX_PARAMETERS)
-				imagefx.num_effect_params = MMAL_MAX_IMAGEFX_PARAMETERS;
-
-			for (j = 0; j < imagefx.num_effect_params; j++)
-				imagefx.effect_parameter[j] =
-					v4l2_to_mmal_effects_values[i].effect_params[j];
-
-			dev->colourfx.enable =
-				v4l2_to_mmal_effects_values[i].col_fx_enable;
-			if (!v4l2_to_mmal_effects_values[i].col_fx_fixed_cbcr) {
-				dev->colourfx.u =
-					v4l2_to_mmal_effects_values[i].u;
-				dev->colourfx.v =
-					v4l2_to_mmal_effects_values[i].v;
-			}
+		if (ctrl->val != v4l2_to_mmal_effects_values[i].v4l2_effect)
+			continue;
+
+		imagefx.effect =
+			v4l2_to_mmal_effects_values[i].mmal_effect;
+		imagefx.num_effect_params =
+			v4l2_to_mmal_effects_values[i].num_effect_params;
 
-			control = &dev->component[COMP_CAMERA]->control;
+		if (imagefx.num_effect_params > MMAL_MAX_IMAGEFX_PARAMETERS)
+			imagefx.num_effect_params = MMAL_MAX_IMAGEFX_PARAMETERS;
 
-			ret = vchiq_mmal_port_parameter_set(
-					dev->instance, control,
-					MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
-					&imagefx, sizeof(imagefx));
-			if (ret)
-				goto exit;
+		for (j = 0; j < imagefx.num_effect_params; j++)
+			imagefx.effect_parameter[j] =
+				v4l2_to_mmal_effects_values[i].effect_params[j];
 
-			ret = vchiq_mmal_port_parameter_set(
-					dev->instance, control,
-					MMAL_PARAMETER_COLOUR_EFFECT,
-					&dev->colourfx, sizeof(dev->colourfx));
+		dev->colourfx.enable =
+			v4l2_to_mmal_effects_values[i].col_fx_enable;
+		if (!v4l2_to_mmal_effects_values[i].col_fx_fixed_cbcr) {
+			dev->colourfx.u = v4l2_to_mmal_effects_values[i].u;
+			dev->colourfx.v = v4l2_to_mmal_effects_values[i].v;
 		}
+
+		control = &dev->component[COMP_CAMERA]->control;
+
+		ret = vchiq_mmal_port_parameter_set(
+				dev->instance, control,
+				MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
+				&imagefx, sizeof(imagefx));
+		if (ret)
+			goto exit;
+
+		ret = vchiq_mmal_port_parameter_set(
+				dev->instance, control,
+				MMAL_PARAMETER_COLOUR_EFFECT,
+				&dev->colourfx, sizeof(dev->colourfx));
 	}
 
 exit:
@@ -841,8 +838,7 @@ static int ctrl_set_scene_mode(struct bm2835_mmal_dev *dev,
 		enum mmal_parameter_exposuremeteringmode metering_mode;
 
 		for (i = 0; i < ARRAY_SIZE(scene_configs); i++) {
-			if (scene_configs[i].v4l2_scene ==
-				ctrl->val) {
+			if (scene_configs[i].v4l2_scene == ctrl->val) {
 				scene = &scene_configs[i];
 				break;
 			}
@@ -1045,8 +1041,8 @@ static const struct bm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
 	{
 		.id = V4L2_CID_EXPOSURE_METERING,
 		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~0x7,
-		.max = V4L2_EXPOSURE_METERING_SPOT,
+		.min = ~0xf,
+		.max = V4L2_EXPOSURE_METERING_MATRIX,
 		.def = V4L2_EXPOSURE_METERING_AVERAGE,
 		.step = 0,
 		.imenu = NULL,
@@ -1282,21 +1278,18 @@ int set_framerate_params(struct bm2835_mmal_dev *dev)
 	struct mmal_parameter_fps_range fps_range;
 	int ret;
 
+	fps_range.fps_high.num = dev->capture.timeperframe.denominator;
+	fps_range.fps_high.den = dev->capture.timeperframe.numerator;
+
 	if ((dev->exposure_mode_active != MMAL_PARAM_EXPOSUREMODE_OFF) &&
 	    (dev->exp_auto_priority)) {
-		/* Variable FPS. Define min FPS as 1fps.
-		 * Max as max defined FPS.
-		 */
+		/* Variable FPS. Define min FPS as 1fps. */
 		fps_range.fps_low.num = 1;
 		fps_range.fps_low.den = 1;
-		fps_range.fps_high.num = dev->capture.timeperframe.denominator;
-		fps_range.fps_high.den = dev->capture.timeperframe.numerator;
 	} else {
 		/* Fixed FPS - set min and max to be the same */
-		fps_range.fps_low.num = fps_range.fps_high.num =
-			dev->capture.timeperframe.denominator;
-		fps_range.fps_low.den = fps_range.fps_high.den =
-			dev->capture.timeperframe.numerator;
+		fps_range.fps_low.num = fps_range.fps_high.num;
+		fps_range.fps_low.den = fps_range.fps_high.den;
 	}
 
 	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
diff --git a/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h
index ff5398737b4a..ce88fac7c24b 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-common.h
@@ -26,13 +26,13 @@ struct mmal_msg_context;
 
 /* mapping between v4l and mmal video modes */
 struct mmal_fmt {
-	u32   fourcc;          /* v4l2 format id */
-	int   flags;           /* v4l2 flags field */
-	u32   mmal;
-	int   depth;
-	u32   mmal_component;  /* MMAL component index to be used to encode */
-	u32   ybbp;            /* depth of first Y plane for planar formats */
-	bool  remove_padding;  /* Does the GPU have to remove padding,
+	u32 fourcc;          /* v4l2 format id */
+	int flags;           /* v4l2 flags field */
+	u32 mmal;
+	int depth;
+	u32 mmal_component;  /* MMAL component index to be used to encode */
+	u32 ybbp;            /* depth of first Y plane for planar formats */
+	bool remove_padding;   /* Does the GPU have to remove padding,
 				* or can we do hide padding via bytesperline.
 				*/
 };
@@ -40,10 +40,10 @@ struct mmal_fmt {
 /* buffer for one video frame */
 struct mmal_buffer {
 	/* v4l buffer data -- must be first */
-	struct vb2_v4l2_buffer	vb;
+	struct vb2_v4l2_buffer vb;
 
 	/* list of buffers available */
-	struct list_head	list;
+	struct list_head list;
 
 	void *buffer; /* buffer pointer */
 	unsigned long buffer_size; /* size of allocated buffer */
diff --git a/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h b/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h
index 80a99128f5f3..f4ac5a6149ea 100644
--- a/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h
+++ b/drivers/staging/vc04_services/bcm2835-camera/mmal-parameters.h
@@ -724,11 +724,11 @@ struct mmal_parameter_imagefx_parameters {
 #define MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN 16
 
 struct mmal_parameter_camera_info_camera_t {
-	u32    port_id;
-	u32    max_width;
-	u32    max_height;
-	u32    lens_present;
-	u8     camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN];
+	u32 port_id;
+	u32 max_width;
+	u32 max_height;
+	u32 lens_present;
+	u8 camera_name[MMAL_PARAMETER_CAMERA_INFO_MAX_STR_LEN];
 };
 
 enum mmal_parameter_camera_info_flash_type_t {
@@ -744,8 +744,8 @@ struct mmal_parameter_camera_info_flash_t {
 };
 
 struct mmal_parameter_camera_info_t {
-	u32                            num_cameras;
-	u32                            num_flashes;
+	u32 num_cameras;
+	u32 num_flashes;
 	struct mmal_parameter_camera_info_camera_t
 				cameras[MMAL_PARAMETER_CAMERA_INFO_MAX_CAMERAS];
 	struct mmal_parameter_camera_info_flash_t
diff --git a/drivers/staging/vc04_services/interface/vchi/vchi.h b/drivers/staging/vc04_services/interface/vchi/vchi.h
index ff2b960d8cac..1a981e98e82b 100644
--- a/drivers/staging/vc04_services/interface/vchi/vchi.h
+++ b/drivers/staging/vc04_services/interface/vchi/vchi.h
@@ -60,32 +60,18 @@ struct vchi_service_handle;
  * (local / remote)
  *****************************************************************************/
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 // Routine used to initialise the vchi on both local + remote connections
 extern int32_t vchi_initialise(struct vchi_instance_handle **instance_handle);
 
-extern int32_t vchi_exit(void);
-
 extern int32_t vchi_connect(struct vchi_instance_handle *instance_handle);
 
 //When this is called, ensure that all services have no data pending.
 //Bulk transfers can remain 'queued'
 extern int32_t vchi_disconnect(struct vchi_instance_handle *instance_handle);
 
-// helper functions
-extern void *vchi_allocate_buffer(struct vchi_service_handle *handle, uint32_t *length);
-extern void vchi_free_buffer(struct vchi_service_handle *handle, void *address);
-extern uint32_t vchi_current_time(struct vchi_instance_handle *instance_handle);
-
 /******************************************************************************
  * Global service API
  *****************************************************************************/
-// Routine to destroy a service
-extern int32_t vchi_service_destroy(const struct vchi_service_handle *handle);
-
 // Routine to open a named service
 extern int32_t vchi_service_open(struct vchi_instance_handle *instance_handle,
 				 struct service_creation *setup,
@@ -103,23 +89,12 @@ extern int32_t vchi_service_use(const struct vchi_service_handle *handle);
 // Routine to decrement ref count on a named service
 extern int32_t vchi_service_release(const struct vchi_service_handle *handle);
 
-// Routine to set a control option for a named service
-extern int32_t vchi_service_set_option(const struct vchi_service_handle *handle,
-				       enum vchi_service_option option,
-				       int value);
-
 /* Routine to send a message from kernel memory across a service */
 extern int
 vchi_queue_kernel_message(struct vchi_service_handle *handle,
 			  void *data,
 			  unsigned int size);
 
-/* Routine to send a message from user memory across a service */
-extern int
-vchi_queue_user_message(struct vchi_service_handle *handle,
-			void __user *data,
-			unsigned int size);
-
 // Routine to receive a msg from a service
 // Dequeue is equivalent to hold, copy into client buffer, release
 extern int32_t vchi_msg_dequeue(struct vchi_service_handle *handle,
@@ -149,54 +124,14 @@ extern int32_t vchi_msg_hold(struct vchi_service_handle *handle,
 			     enum vchi_flags flags,
 			     struct vchi_held_msg *message_descriptor);
 
-// Initialise an iterator to look through messages in place
-extern int32_t vchi_msg_look_ahead(struct vchi_service_handle *handle,
-				   struct vchi_msg_iter *iter,
-				   enum vchi_flags flags);
-
 /*******************************************************************************
  * Global service support API - operations on held messages
  * and message iterators
  ******************************************************************************/
 
-// Routine to get the address of a held message
-extern void *vchi_held_msg_ptr(const struct vchi_held_msg *message);
-
-// Routine to get the size of a held message
-extern int32_t vchi_held_msg_size(const struct vchi_held_msg *message);
-
-// Routine to get the transmit timestamp as written into the header by the peer
-extern uint32_t vchi_held_msg_tx_timestamp(const struct vchi_held_msg *message);
-
-// Routine to get the reception timestamp, written as we parsed the header
-extern uint32_t vchi_held_msg_rx_timestamp(const struct vchi_held_msg *message);
-
 // Routine to release a held message after it has been processed
 extern int32_t vchi_held_msg_release(struct vchi_held_msg *message);
 
-// Indicates whether the iterator has a next message.
-extern int32_t vchi_msg_iter_has_next(const struct vchi_msg_iter *iter);
-
-// Return the pointer and length for the next message and advance the iterator.
-extern int32_t vchi_msg_iter_next(struct vchi_msg_iter *iter,
-				  void **data,
-				  uint32_t *msg_size);
-
-// Remove the last message returned by vchi_msg_iter_next.
-// Can only be called once after each call to vchi_msg_iter_next.
-extern int32_t vchi_msg_iter_remove(struct vchi_msg_iter *iter);
-
-// Hold the last message returned by vchi_msg_iter_next.
-// Can only be called once after each call to vchi_msg_iter_next.
-extern int32_t vchi_msg_iter_hold(struct vchi_msg_iter *iter,
-				  struct vchi_held_msg *message);
-
-// Return information for the next message, and hold it, advancing the iterator.
-extern int32_t vchi_msg_iter_hold_next(struct vchi_msg_iter *iter,
-				       void **data,        // } may be NULL
-				       uint32_t *msg_size, // }
-				       struct vchi_held_msg *message);
-
 /******************************************************************************
  * Global bulk API
  *****************************************************************************/
@@ -208,13 +143,6 @@ extern int32_t vchi_bulk_queue_receive(struct vchi_service_handle *handle,
 				       enum vchi_flags flags,
 				       void *transfer_handle);
 
-// Prepare interface for a transfer from the other side into relocatable memory.
-int32_t vchi_bulk_queue_receive_reloc(const struct vchi_service_handle *handle,
-				      uint32_t offset,
-				      uint32_t data_size,
-				      const enum vchi_flags flags,
-				      void * const bulk_handle);
-
 // Routine to queue up data ready for transfer to the other (once they have signalled they are ready)
 extern int32_t vchi_bulk_queue_transmit(struct vchi_service_handle *handle,
 					const void *data_src,
@@ -226,15 +154,6 @@ extern int32_t vchi_bulk_queue_transmit(struct vchi_service_handle *handle,
  * Configuration plumbing
  *****************************************************************************/
 
-#ifdef __cplusplus
-}
-#endif
-
-extern int32_t vchi_bulk_queue_transmit_reloc(struct vchi_service_handle *handle,
-					      uint32_t offset,
-					      uint32_t data_size,
-					      enum vchi_flags flags,
-					      void *transfer_handle);
 #endif /* VCHI_H_ */
 
 /****************************** End of file **********************************/
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 c18c6ca0b6c0..38a13e4618a8 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
@@ -371,14 +371,15 @@ create_pagelist(char __user *buf, size_t count, unsigned short type)
 	pagelistinfo->scatterlist = scatterlist;
 	pagelistinfo->scatterlist_mapped = 0;
 
-	if (is_vmalloc_addr(buf)) {
+	if (is_vmalloc_addr((void __force *)buf)) {
 		unsigned long length = count;
 		unsigned int off = offset;
 
 		for (actual_pages = 0; actual_pages < num_pages;
 		     actual_pages++) {
-			struct page *pg = vmalloc_to_page(buf + (actual_pages *
-								 PAGE_SIZE));
+			struct page *pg =
+				vmalloc_to_page((void __force *)(buf +
+						 (actual_pages * PAGE_SIZE)));
 			size_t bytes = PAGE_SIZE - off;
 
 			if (!pg) {
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 a1ea9777a444..28ea8c3a4cba 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
@@ -1209,7 +1209,9 @@ vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 					/* The completion must point to the
 					** msgbuf. */
-					completion->header = msgbuf;
+					completion->header =
+						(struct vchiq_header __force *)
+						msgbuf;
 				}
 
 				if ((completion->reason ==
@@ -2353,7 +2355,7 @@ vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service,
 	enum vchiq_status ret = VCHIQ_SUCCESS;
 	char entity[16];
 	int *entity_uc;
-	int local_uc, local_entity_uc;
+	int local_uc;
 
 	if (!arm_state)
 		goto out;
@@ -2377,7 +2379,7 @@ vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service,
 
 	write_lock_bh(&arm_state->susp_res_lock);
 	local_uc = ++arm_state->videocore_use_count;
-	local_entity_uc = ++(*entity_uc);
+	++(*entity_uc);
 
 	vchiq_log_trace(vchiq_susp_log_level,
 		"%s %s count %d, state count %d",
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
index 1640906e3929..79b75efa6868 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_connected.c
@@ -14,12 +14,7 @@ static   VCHIQ_CONNECTED_CALLBACK_T g_deferred_callback[MAX_CALLBACKS];
 static   int                        g_once_init;
 static   struct mutex               g_connected_mutex;
 
-/****************************************************************************
-*
-* Function to initialize our lock.
-*
-***************************************************************************/
-
+/* Function to initialize our lock */
 static void connected_init(void)
 {
 	if (!g_once_init) {
@@ -28,15 +23,12 @@ static void connected_init(void)
 	}
 }
 
-/****************************************************************************
-*
-* This function is used to defer initialization until the vchiq stack is
-* initialized. If the stack is already initialized, then the callback will
-* be made immediately, otherwise it will be deferred until
-* vchiq_call_connected_callbacks is called.
-*
-***************************************************************************/
-
+/*
+ * This function is used to defer initialization until the vchiq stack is
+ * initialized. If the stack is already initialized, then the callback will
+ * be made immediately, otherwise it will be deferred until
+ * vchiq_call_connected_callbacks is called.
+ */
 void vchiq_add_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback)
 {
 	connected_init();
@@ -63,13 +55,10 @@ void vchiq_add_connected_callback(VCHIQ_CONNECTED_CALLBACK_T callback)
 	mutex_unlock(&g_connected_mutex);
 }
 
-/****************************************************************************
-*
-* This function is called by the vchiq stack once it has been connected to
-* the videocore and clients can start to use the stack.
-*
-***************************************************************************/
-
+/*
+ * This function is called by the vchiq stack once it has been connected to
+ * the videocore and clients can start to use the stack.
+ */
 void vchiq_call_connected_callbacks(void)
 {
 	int i;
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 edcd97373809..ae9183db44ee 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
@@ -372,6 +372,10 @@ make_service_callback(struct vchiq_service *service, enum vchiq_reason reason,
 			service->state->id, service->handle);
 		status = VCHIQ_SUCCESS;
 	}
+
+	if (reason != VCHIQ_MESSAGE_AVAILABLE)
+		vchiq_release_message(service->handle, header);
+
 	return status;
 }
 
@@ -1480,15 +1484,6 @@ parse_open(struct vchiq_state *state, struct vchiq_header *header)
 					: VCHIQ_SRVSTATE_OPEN);
 			}
 
-			service->remoteport = remoteport;
-			service->client_id = ((int *)header->data)[1];
-			if (make_service_callback(service, VCHIQ_SERVICE_OPENED,
-				NULL, NULL) == VCHIQ_RETRY) {
-				/* Bail out if not ready */
-				service->remoteport = VCHIQ_PORT_FREE;
-				goto bail_not_ready;
-			}
-
 			/* Success - the message has been dealt with */
 			unlock_service(service);
 			return 1;
@@ -3147,6 +3142,12 @@ error_exit:
 	return status;
 }
 
+enum vchiq_status vchiq_queue_kernel_message(unsigned int handle, void *context,
+				      size_t size)
+{
+	return vchiq_queue_message(handle, memcpy_copy_callback, context, size);
+}
+
 void
 vchiq_release_message(unsigned int handle,
 		      struct vchiq_header *header)
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
index cedd8e721aae..1fe6cd8b86c0 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
@@ -587,6 +587,13 @@ lock_service(struct vchiq_service *service);
 extern void
 unlock_service(struct vchiq_service *service);
 
+extern enum vchiq_status
+vchiq_queue_message(unsigned int handle,
+		    ssize_t (*copy_callback)(void *context, void *dest,
+					     size_t offset, size_t maxsize),
+		    void *context,
+		    size_t size);
+
 /* The following functions are called from vchiq_core, and external
 ** implementations must be provided. */
 
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 39b77ea19210..b62fd6d6f1ac 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_if.h
@@ -105,12 +105,8 @@ extern enum vchiq_status vchiq_close_service(unsigned int service);
 extern enum vchiq_status vchiq_remove_service(unsigned int service);
 extern enum vchiq_status vchiq_use_service(unsigned int service);
 extern enum vchiq_status vchiq_release_service(unsigned int service);
-extern enum vchiq_status
-vchiq_queue_message(unsigned int handle,
-		    ssize_t (*copy_callback)(void *context, void *dest,
-					     size_t offset, size_t maxsize),
-		    void *context,
-		    size_t size);
+extern enum vchiq_status vchiq_queue_kernel_message(unsigned int handle,
+						    void *context, size_t size);
 extern void           vchiq_release_message(unsigned int service,
 	struct vchiq_header *header);
 extern enum vchiq_status vchiq_bulk_transmit(unsigned int service,
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 efdd3b1c7d85..75d87b6992c4 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c
+++ b/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_shim.c
@@ -9,8 +9,6 @@
 
 #include "vchiq_util.h"
 
-#define vchiq_status_to_vchi(status) ((int32_t)status)
-
 struct shim_service {
 	unsigned int handle;
 
@@ -84,35 +82,15 @@ int32_t vchi_msg_remove(struct vchi_service_handle *handle)
 }
 EXPORT_SYMBOL(vchi_msg_remove);
 
-/***********************************************************
- * Name: vchi_msg_queue
- *
- * Arguments:  struct vchi_service_handle *handle,
- *             ssize_t (*copy_callback)(void *context, void *dest,
- *				        size_t offset, size_t maxsize),
- *	       void *context,
- *             uint32_t data_size
- *
- * Description: Thin wrapper to queue a message onto a connection
- *
- * Returns: int32_t - success == 0
- *
- ***********************************************************/
-static
-int32_t vchi_msg_queue(struct vchi_service_handle *handle,
-	ssize_t (*copy_callback)(void *context, void *dest,
-				 size_t offset, size_t maxsize),
-	void *context,
-	uint32_t data_size)
+int vchi_queue_kernel_message(struct vchi_service_handle *handle, void *data,
+			       unsigned int size)
 {
 	struct shim_service *service = (struct shim_service *)handle;
 	enum vchiq_status status;
 
 	while (1) {
-		status = vchiq_queue_message(service->handle,
-					     copy_callback,
-					     context,
-					     data_size);
+		status = vchiq_queue_kernel_message(service->handle, data,
+						    size);
 
 		/*
 		 * vchiq_queue_message() may return VCHIQ_RETRY, so we need to
@@ -125,65 +103,10 @@ int32_t vchi_msg_queue(struct vchi_service_handle *handle,
 		msleep(1);
 	}
 
-	return vchiq_status_to_vchi(status);
-}
-
-static ssize_t
-vchi_queue_kernel_message_callback(void *context,
-				   void *dest,
-				   size_t offset,
-				   size_t maxsize)
-{
-	memcpy(dest, context + offset, maxsize);
-	return maxsize;
-}
-
-int
-vchi_queue_kernel_message(struct vchi_service_handle *handle,
-			  void *data,
-			  unsigned int size)
-{
-	return vchi_msg_queue(handle,
-			      vchi_queue_kernel_message_callback,
-			      data,
-			      size);
+	return status;
 }
 EXPORT_SYMBOL(vchi_queue_kernel_message);
 
-struct vchi_queue_user_message_context {
-	void __user *data;
-};
-
-static ssize_t
-vchi_queue_user_message_callback(void *context,
-				 void *dest,
-				 size_t offset,
-				 size_t maxsize)
-{
-	struct vchi_queue_user_message_context *copycontext = context;
-
-	if (copy_from_user(dest, copycontext->data + offset, maxsize))
-		return -EFAULT;
-
-	return maxsize;
-}
-
-int
-vchi_queue_user_message(struct vchi_service_handle *handle,
-			void __user *data,
-			unsigned int size)
-{
-	struct vchi_queue_user_message_context copycontext = {
-		.data = data
-	};
-
-	return vchi_msg_queue(handle,
-			      vchi_queue_user_message_callback,
-			      &copycontext,
-			      size);
-}
-EXPORT_SYMBOL(vchi_queue_user_message);
-
 /***********************************************************
  * Name: vchi_bulk_queue_receive
  *
@@ -221,7 +144,7 @@ int32_t vchi_bulk_queue_receive(struct vchi_service_handle *handle, void *data_d
 		break;
 	default:
 		WARN(1, "unsupported message\n");
-		return vchiq_status_to_vchi(VCHIQ_ERROR);
+		return VCHIQ_ERROR;
 	}
 
 	while (1) {
@@ -238,7 +161,7 @@ int32_t vchi_bulk_queue_receive(struct vchi_service_handle *handle, void *data_d
 		msleep(1);
 	}
 
-	return vchiq_status_to_vchi(status);
+	return status;
 }
 EXPORT_SYMBOL(vchi_bulk_queue_receive);
 
@@ -282,7 +205,7 @@ int32_t vchi_bulk_queue_transmit(struct vchi_service_handle *handle,
 		break;
 	default:
 		WARN(1, "unsupported message\n");
-		return vchiq_status_to_vchi(VCHIQ_ERROR);
+		return VCHIQ_ERROR;
 	}
 
 	while (1) {
@@ -300,7 +223,7 @@ int32_t vchi_bulk_queue_transmit(struct vchi_service_handle *handle,
 		msleep(1);
 	}
 
-	return vchiq_status_to_vchi(status);
+	return status;
 }
 EXPORT_SYMBOL(vchi_bulk_queue_transmit);
 
@@ -447,7 +370,7 @@ int32_t vchi_initialise(struct vchi_instance_handle **instance_handle)
 
 	*instance_handle = (struct vchi_instance_handle *)instance;
 
-	return vchiq_status_to_vchi(status);
+	return status;
 }
 EXPORT_SYMBOL(vchi_initialise);
 
@@ -485,7 +408,7 @@ int32_t vchi_disconnect(struct vchi_instance_handle *instance_handle)
 {
 	struct vchiq_instance *instance = (struct vchiq_instance *)instance_handle;
 
-	return vchiq_status_to_vchi(vchiq_shutdown(instance));
+	return vchiq_shutdown(instance);
 }
 EXPORT_SYMBOL(vchi_disconnect);
 
@@ -521,7 +444,7 @@ static enum vchiq_status shim_callback(enum vchiq_reason reason,
 		service->callback(service->callback_param,
 				  VCHI_CALLBACK_MSG_AVAILABLE, NULL);
 
-		goto done;
+		break;
 
 	case VCHIQ_BULK_TRANSMIT_DONE:
 		service->callback(service->callback_param,
@@ -538,10 +461,6 @@ static enum vchiq_status shim_callback(enum vchiq_reason reason,
 				  VCHI_CALLBACK_SERVICE_CLOSED, NULL);
 		break;
 
-	case VCHIQ_SERVICE_OPENED:
-		/* No equivalent VCHI reason */
-		break;
-
 	case VCHIQ_BULK_TRANSMIT_ABORTED:
 		service->callback(service->callback_param,
 				  VCHI_CALLBACK_BULK_TRANSMIT_ABORTED,
@@ -560,8 +479,6 @@ static enum vchiq_status shim_callback(enum vchiq_reason reason,
 	}
 
 release:
-	vchiq_release_message(service->handle, header);
-done:
 	return VCHIQ_SUCCESS;
 }
 
@@ -636,62 +553,12 @@ int32_t vchi_service_close(const struct vchi_service_handle *handle)
 		if (status == VCHIQ_SUCCESS)
 			service_free(service);
 
-		ret = vchiq_status_to_vchi(status);
+		ret = status;
 	}
 	return ret;
 }
 EXPORT_SYMBOL(vchi_service_close);
 
-int32_t vchi_service_destroy(const struct vchi_service_handle *handle)
-{
-	int32_t ret = -1;
-	struct shim_service *service = (struct shim_service *)handle;
-
-	if (service) {
-		enum vchiq_status status = vchiq_remove_service(service->handle);
-
-		if (status == VCHIQ_SUCCESS) {
-			service_free(service);
-			service = NULL;
-		}
-
-		ret = vchiq_status_to_vchi(status);
-	}
-	return ret;
-}
-EXPORT_SYMBOL(vchi_service_destroy);
-
-int32_t vchi_service_set_option(const struct vchi_service_handle *handle,
-				enum vchi_service_option option,
-				int value)
-{
-	int32_t ret = -1;
-	struct shim_service *service = (struct shim_service *)handle;
-	enum vchiq_service_option vchiq_option;
-
-	switch (option) {
-	case VCHI_SERVICE_OPTION_TRACE:
-		vchiq_option = VCHIQ_SERVICE_OPTION_TRACE;
-		break;
-	case VCHI_SERVICE_OPTION_SYNCHRONOUS:
-		vchiq_option = VCHIQ_SERVICE_OPTION_SYNCHRONOUS;
-		break;
-	default:
-		service = NULL;
-		break;
-	}
-	if (service) {
-		enum vchiq_status status =
-			vchiq_set_service_option(service->handle,
-						vchiq_option,
-						value);
-
-		ret = vchiq_status_to_vchi(status);
-	}
-	return ret;
-}
-EXPORT_SYMBOL(vchi_service_set_option);
-
 int32_t vchi_get_peer_version(const struct vchi_service_handle *handle, short *peer_version)
 {
 	int32_t ret = -1;
@@ -701,7 +568,7 @@ int32_t vchi_get_peer_version(const struct vchi_service_handle *handle, short *p
 		enum vchiq_status status;
 
 		status = vchiq_get_peer_version(service->handle, peer_version);
-		ret = vchiq_status_to_vchi(status);
+		ret = status;
 	}
 	return ret;
 }
@@ -723,7 +590,7 @@ int32_t vchi_service_use(const struct vchi_service_handle *handle)
 
 	struct shim_service *service = (struct shim_service *)handle;
 	if (service)
-		ret = vchiq_status_to_vchi(vchiq_use_service(service->handle));
+		ret = vchiq_use_service(service->handle);
 	return ret;
 }
 EXPORT_SYMBOL(vchi_service_use);
@@ -744,8 +611,7 @@ int32_t vchi_service_release(const struct vchi_service_handle *handle)
 
 	struct shim_service *service = (struct shim_service *)handle;
 	if (service)
-		ret = vchiq_status_to_vchi(
-			vchiq_release_service(service->handle));
+		ret = vchiq_release_service(service->handle);
 	return ret;
 }
 EXPORT_SYMBOL(vchi_service_release);
diff --git a/drivers/staging/vt6655/Makefile b/drivers/staging/vt6655/Makefile
index a151f30fc46f..e70357ec0af8 100644
--- a/drivers/staging/vt6655/Makefile
+++ b/drivers/staging/vt6655/Makefile
@@ -1,7 +1,4 @@
 # SPDX-License-Identifier: GPL-2.0
-# TODO: all of these should be removed
-ccflags-y := -DLINUX -D__KERNEL__ -D__NO_VERSION__
-ccflags-y += -DHOSTAP
 
 vt6655_stage-y +=	device_main.o \
 	card.o \
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index b4cdc0b7fee7..6b25d75d2501 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -12,12 +12,10 @@
  * Date: Aug.22, 2002
  *
  * Functions:
- *      BBuGetFrameTime        - Calculate data frame transmitting time
- *      BBvCalculateParameter   - Calculate PhyLength, PhyService and Phy Signal
- *                               parameter for baseband Tx
- *      BBbReadEmbedded         - Embedded read baseband register via MAC
- *      BBbWriteEmbedded        - Embedded write baseband register via MAC
- *      BBbVT3253Init          - VIA VT3253 baseband chip init code
+ *      bb_get_frame_time	 - Calculate data frame transmitting time
+ *      bb_read_embedded	 - Embedded read baseband register via MAC
+ *      bb_write_embedded	 - Embedded write baseband register via MAC
+ *      bb_vt3253_init		 - VIA VT3253 baseband chip init code
  *
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
@@ -1695,53 +1693,53 @@ static const unsigned short awcFrameTime[MAX_RATE] = {
  *
  * Parameters:
  *  In:
- *      byPreambleType  - Preamble Type
- *      byPktType        - PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
- *      cbFrameLength   - Baseband Type
- *      wRate           - Tx Rate
+ *      by_preamble_type  - Preamble Type
+ *      by_pkt_type        - PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
+ *      cb_frame_length   - Baseband Type
+ *      tx_rate           - Tx Rate
  *  Out:
  *
  * Return Value: FrameTime
  *
  */
-unsigned int BBuGetFrameTime(unsigned char byPreambleType,
-			     unsigned char byPktType,
-			     unsigned int cbFrameLength, unsigned short wRate)
+unsigned int bb_get_frame_time(unsigned char by_preamble_type,
+			       unsigned char by_pkt_type,
+			       unsigned int cb_frame_length,
+			       unsigned short tx_rate)
 {
-	unsigned int uFrameTime;
-	unsigned int uPreamble;
-	unsigned int uTmp;
-	unsigned int uRateIdx = (unsigned int)wRate;
-	unsigned int uRate = 0;
+	unsigned int frame_time;
+	unsigned int preamble;
+	unsigned int tmp;
+	unsigned int rate_idx = (unsigned int)tx_rate;
+	unsigned int rate = 0;
 
-	if (uRateIdx > RATE_54M)
+	if (rate_idx > RATE_54M)
 		return 0;
 
-	uRate = (unsigned int)awcFrameTime[uRateIdx];
+	rate = (unsigned int)awcFrameTime[rate_idx];
 
-	if (uRateIdx <= 3) {          /* CCK mode */
-		if (byPreambleType == 1) /* Short */
-			uPreamble = 96;
+	if (rate_idx <= 3) {		    /* CCK mode */
+		if (by_preamble_type == 1) /* Short */
+			preamble = 96;
 		else
-			uPreamble = 192;
-
-		uFrameTime = (cbFrameLength * 80) / uRate;  /* ????? */
-		uTmp = (uFrameTime * uRate) / 80;
-		if (cbFrameLength != uTmp)
-			uFrameTime++;
+			preamble = 192;
+		frame_time = (cb_frame_length * 80) / rate;  /* ????? */
+		tmp = (frame_time * rate) / 80;
+		if (cb_frame_length != tmp)
+			frame_time++;
 
-		return uPreamble + uFrameTime;
+		return preamble + frame_time;
 	}
-	uFrameTime = (cbFrameLength * 8 + 22) / uRate; /* ???????? */
-	uTmp = ((uFrameTime * uRate) - 22) / 8;
-	if (cbFrameLength != uTmp)
-		uFrameTime++;
+	frame_time = (cb_frame_length * 8 + 22) / rate; /* ???????? */
+	tmp = ((frame_time * rate) - 22) / 8;
+	if (cb_frame_length != tmp)
+		frame_time++;
 
-	uFrameTime = uFrameTime * 4;    /* ??????? */
-	if (byPktType != PK_TYPE_11A)
-		uFrameTime += 6;     /* ?????? */
+	frame_time = frame_time * 4;    /* ??????? */
+	if (by_pkt_type != PK_TYPE_11A)
+		frame_time += 6;     /* ?????? */
 
-	return 20 + uFrameTime; /* ?????? */
+	return 20 + frame_time; /* ?????? */
 }
 
 /*
@@ -1899,34 +1897,34 @@ void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
  * Parameters:
  *  In:
  *      iobase      - I/O base address
- *      byBBAddr    - address of register in Baseband
+ *      by_bb_addr  - address of register in Baseband
  *  Out:
- *      pbyData     - data read
+ *      pby_data    - data read
  *
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbReadEmbedded(struct vnt_private *priv,
-		     unsigned char byBBAddr, unsigned char *pbyData)
+bool bb_read_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		      unsigned char *pby_data)
 {
 	void __iomem *iobase = priv->PortOffset;
 	unsigned short ww;
-	unsigned char byValue;
+	unsigned char by_value;
 
 	/* BB reg offset */
-	VNSvOutPortB(iobase + MAC_REG_BBREGADR, byBBAddr);
+	VNSvOutPortB(iobase + MAC_REG_BBREGADR, by_bb_addr);
 
 	/* turn on REGR */
 	MACvRegBitsOn(iobase, MAC_REG_BBREGCTL, BBREGCTL_REGR);
 	/* W_MAX_TIMEOUT is the timeout period */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-		VNSvInPortB(iobase + MAC_REG_BBREGCTL, &byValue);
-		if (byValue & BBREGCTL_DONE)
+		VNSvInPortB(iobase + MAC_REG_BBREGCTL, &by_value);
+		if (by_value & BBREGCTL_DONE)
 			break;
 	}
 
 	/* get BB data */
-	VNSvInPortB(iobase + MAC_REG_BBREGDATA, pbyData);
+	VNSvInPortB(iobase + MAC_REG_BBREGDATA, pby_data);
 
 	if (ww == W_MAX_TIMEOUT) {
 		pr_debug(" DBG_PORT80(0x30)\n");
@@ -1941,32 +1939,32 @@ bool BBbReadEmbedded(struct vnt_private *priv,
  * Parameters:
  *  In:
  *      iobase      - I/O base address
- *      byBBAddr    - address of register in Baseband
- *      byData      - data to write
+ *      by_bb_addr  - address of register in Baseband
+ *      by_data     - data to write
  *  Out:
  *      none
  *
  * Return Value: true if succeeded; false if failed.
  *
  */
-bool BBbWriteEmbedded(struct vnt_private *priv,
-		      unsigned char byBBAddr, unsigned char byData)
+bool bb_write_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		       unsigned char by_data)
 {
 	void __iomem *iobase = priv->PortOffset;
 	unsigned short ww;
-	unsigned char byValue;
+	unsigned char by_value;
 
 	/* BB reg offset */
-	VNSvOutPortB(iobase + MAC_REG_BBREGADR, byBBAddr);
+	VNSvOutPortB(iobase + MAC_REG_BBREGADR, by_bb_addr);
 	/* set BB data */
-	VNSvOutPortB(iobase + MAC_REG_BBREGDATA, byData);
+	VNSvOutPortB(iobase + MAC_REG_BBREGDATA, by_data);
 
 	/* turn on BBREGCTL_REGW */
 	MACvRegBitsOn(iobase, MAC_REG_BBREGCTL, BBREGCTL_REGW);
 	/* W_MAX_TIMEOUT is the timeout period */
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
-		VNSvInPortB(iobase + MAC_REG_BBREGCTL, &byValue);
-		if (byValue & BBREGCTL_DONE)
+		VNSvInPortB(iobase + MAC_REG_BBREGCTL, &by_value);
+		if (by_value & BBREGCTL_DONE)
 			break;
 	}
 
@@ -1992,29 +1990,29 @@ bool BBbWriteEmbedded(struct vnt_private *priv,
  *
  */
 
-bool BBbVT3253Init(struct vnt_private *priv)
+bool bb_vt3253_init(struct vnt_private *priv)
 {
-	bool bResult = true;
+	bool result = true;
 	int        ii;
 	void __iomem *iobase = priv->PortOffset;
-	unsigned char byRFType = priv->byRFType;
-	unsigned char byLocalID = priv->byLocalID;
+	unsigned char by_rf_type = priv->byRFType;
+	unsigned char by_local_id = priv->byLocalID;
 
-	if (byRFType == RF_RFMD2959) {
-		if (byLocalID <= REV_ID_VT3253_A1) {
+	if (by_rf_type == RF_RFMD2959) {
+		if (by_local_id <= REV_ID_VT3253_A1) {
 			for (ii = 0; ii < CB_VT3253_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(priv,
+				result &= bb_write_embedded(priv,
 					byVT3253InitTab_RFMD[ii][0],
 					byVT3253InitTab_RFMD[ii][1]);
 
 		} else {
 			for (ii = 0; ii < CB_VT3253B0_INIT_FOR_RFMD; ii++)
-				bResult &= BBbWriteEmbedded(priv,
+				result &= bb_write_embedded(priv,
 					byVT3253B0_RFMD[ii][0],
 					byVT3253B0_RFMD[ii][1]);
 
 			for (ii = 0; ii < CB_VT3253B0_AGC_FOR_RFMD2959; ii++)
-				bResult &= BBbWriteEmbedded(priv,
+				result &= bb_write_embedded(priv,
 					byVT3253B0_AGC4_RFMD2959[ii][0],
 					byVT3253B0_AGC4_RFMD2959[ii][1]);
 
@@ -2029,14 +2027,14 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[1] = -50;
 		priv->ldBmThreshold[2] = 0;
 		priv->ldBmThreshold[3] = 0;
-	} else if ((byRFType == RF_AIROHA) || (byRFType == RF_AL2230S)) {
+	} else if ((by_rf_type == RF_AIROHA) || (by_rf_type == RF_AL2230S)) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AIROHA2230[ii][0],
 				byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
@@ -2047,14 +2045,14 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[1] = -48;
 		priv->ldBmThreshold[2] = 0;
 		priv->ldBmThreshold[3] = 0;
-	} else if (byRFType == RF_UW2451) {
+	} else if (by_rf_type == RF_UW2451) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_UW2451[ii][0],
 				byVT3253B0_UW2451[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AGC[ii][0],
 				byVT3253B0_AGC[ii][1]);
 
@@ -2069,9 +2067,9 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[1] = -50;
 		priv->ldBmThreshold[2] = 0;
 		priv->ldBmThreshold[3] = 0;
-	} else if (byRFType == RF_UW2452) {
+	} else if (by_rf_type == RF_UW2452) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_UW2451; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_UW2451[ii][0],
 				byVT3253B0_UW2451[ii][1]);
 
@@ -2080,7 +2078,7 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		 * 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
 		 */
 
-		/*bResult &= BBbWriteEmbedded(iobase,0x09,0x41);*/
+		/*bResult &= bb_write_embedded(iobase,0x09,0x41);*/
 
 		/* Init ANT B select,
 		 * RX Config CR10 = 0x28->0x2A,
@@ -2088,23 +2086,23 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		 * make the ANT_A, ANT_B inverted)
 		 */
 
-		/*bResult &= BBbWriteEmbedded(iobase,0x0a,0x28);*/
+		/*bResult &= bb_write_embedded(iobase,0x0a,0x28);*/
 		/* Select VC1/VC2, CR215 = 0x02->0x06 */
-		bResult &= BBbWriteEmbedded(priv, 0xd7, 0x06);
+		result &= bb_write_embedded(priv, 0xd7, 0x06);
 
 		/* {{RobertYu:20050125, request by Jack */
-		bResult &= BBbWriteEmbedded(priv, 0x90, 0x20);
-		bResult &= BBbWriteEmbedded(priv, 0x97, 0xeb);
+		result &= bb_write_embedded(priv, 0x90, 0x20);
+		result &= bb_write_embedded(priv, 0x97, 0xeb);
 		/* }} */
 
 		/* {{RobertYu:20050221, request by Jack */
-		bResult &= BBbWriteEmbedded(priv, 0xa6, 0x00);
-		bResult &= BBbWriteEmbedded(priv, 0xa8, 0x30);
+		result &= bb_write_embedded(priv, 0xa6, 0x00);
+		result &= bb_write_embedded(priv, 0xa8, 0x30);
 		/* }} */
-		bResult &= BBbWriteEmbedded(priv, 0xb0, 0x58);
+		result &= bb_write_embedded(priv, 0xb0, 0x58);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x14;
@@ -2117,14 +2115,14 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->ldBmThreshold[3] = 0;
 		/* }} RobertYu */
 
-	} else if (byRFType == RF_VT3226) {
+	} else if (by_rf_type == RF_VT3226) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AIROHA2230[ii][0],
 				byVT3253B0_AIROHA2230[ii][1]);
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
@@ -2138,9 +2136,9 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		/* Fix VT3226 DFC system timing issue */
 		MACvSetRFLE_LatchBase(iobase);
 		/* {{ RobertYu: 20050104 */
-	} else if (byRFType == RF_AIROHA7230) {
+	} else if (by_rf_type == RF_AIROHA7230) {
 		for (ii = 0; ii < CB_VT3253B0_INIT_FOR_AIROHA2230; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AIROHA2230[ii][0],
 				byVT3253B0_AIROHA2230[ii][1]);
 
@@ -2148,17 +2146,17 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		/* Init ANT B select,TX Config CR09 = 0x61->0x45,
 		 * 0x45->0x41(VC1/VC2 define, make the ANT_A, ANT_B inverted)
 		 */
-		/*bResult &= BBbWriteEmbedded(iobase,0x09,0x41);*/
+		/* bResult &= bb_write_embedded(iobase,0x09,0x41);*/
 		/* Init ANT B select,RX Config CR10 = 0x28->0x2A,
 		 * 0x2A->0x28(VC1/VC2 define, make the ANT_A, ANT_B inverted)
 		 */
-		/*bResult &= BBbWriteEmbedded(iobase,0x0a,0x28);*/
+		/* bResult &= BBbWriteEmbedded(iobase,0x0a,0x28);*/
 		/* Select VC1/VC2, CR215 = 0x02->0x06 */
-		bResult &= BBbWriteEmbedded(priv, 0xd7, 0x06);
+		result &= bb_write_embedded(priv, 0xd7, 0x06);
 		/* }} */
 
 		for (ii = 0; ii < CB_VT3253B0_AGC; ii++)
-			bResult &= BBbWriteEmbedded(priv,
+			result &= bb_write_embedded(priv,
 				byVT3253B0_AGC[ii][0], byVT3253B0_AGC[ii][1]);
 
 		priv->abyBBVGA[0] = 0x1C;
@@ -2176,12 +2174,12 @@ bool BBbVT3253Init(struct vnt_private *priv)
 		priv->abyBBVGA[0] = 0x1C;
 	}
 
-	if (byLocalID > REV_ID_VT3253_A1) {
-		BBbWriteEmbedded(priv, 0x04, 0x7F);
-		BBbWriteEmbedded(priv, 0x0D, 0x01);
+	if (by_local_id > REV_ID_VT3253_A1) {
+		bb_write_embedded(priv, 0x04, 0x7F);
+		bb_write_embedded(priv, 0x0D, 0x01);
 	}
 
-	return bResult;
+	return result;
 }
 
 /*
@@ -2197,42 +2195,42 @@ bool BBbVT3253Init(struct vnt_private *priv)
  *
  */
 void
-BBvSetShortSlotTime(struct vnt_private *priv)
+bb_set_short_slot_time(struct vnt_private *priv)
 {
-	unsigned char byBBRxConf = 0;
-	unsigned char byBBVGA = 0;
+	unsigned char by_bb_rx_conf = 0;
+	unsigned char by_bb_vga = 0;
 
-	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
 
 	if (priv->bShortSlotTime)
-		byBBRxConf &= 0xDF; /* 1101 1111 */
+		by_bb_rx_conf &= 0xDF; /* 1101 1111 */
 	else
-		byBBRxConf |= 0x20; /* 0010 0000 */
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
 
 	/* patch for 3253B0 Baseband with Cardbus module */
-	BBbReadEmbedded(priv, 0xE7, &byBBVGA);
-	if (byBBVGA == priv->abyBBVGA[0])
-		byBBRxConf |= 0x20; /* 0010 0000 */
+	bb_read_embedded(priv, 0xE7, &by_bb_vga);
+	if (by_bb_vga == priv->abyBBVGA[0])
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
 
-	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
 }
 
-void BBvSetVGAGainOffset(struct vnt_private *priv, unsigned char byData)
+void bb_set_vga_gain_offset(struct vnt_private *priv, unsigned char by_data)
 {
-	unsigned char byBBRxConf = 0;
+	unsigned char by_bb_rx_conf = 0;
 
-	BBbWriteEmbedded(priv, 0xE7, byData);
+	bb_write_embedded(priv, 0xE7, by_data);
 
-	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
 	/* patch for 3253B0 Baseband with Cardbus module */
-	if (byData == priv->abyBBVGA[0])
-		byBBRxConf |= 0x20; /* 0010 0000 */
+	if (by_data == priv->abyBBVGA[0])
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
 	else if (priv->bShortSlotTime)
-		byBBRxConf &= 0xDF; /* 1101 1111 */
+		by_bb_rx_conf &= 0xDF; /* 1101 1111 */
 	else
-		byBBRxConf |= 0x20; /* 0010 0000 */
-	priv->byBBVGACurrent = byData;
-	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
+		by_bb_rx_conf |= 0x20; /* 0010 0000 */
+	priv->byBBVGACurrent = by_data;
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
 }
 
 /*
@@ -2248,12 +2246,12 @@ void BBvSetVGAGainOffset(struct vnt_private *priv, unsigned char byData)
  *
  */
 void
-BBvSoftwareReset(struct vnt_private *priv)
+bb_software_reset(struct vnt_private *priv)
 {
-	BBbWriteEmbedded(priv, 0x50, 0x40);
-	BBbWriteEmbedded(priv, 0x50, 0);
-	BBbWriteEmbedded(priv, 0x9C, 0x01);
-	BBbWriteEmbedded(priv, 0x9C, 0);
+	bb_write_embedded(priv, 0x50, 0x40);
+	bb_write_embedded(priv, 0x50, 0);
+	bb_write_embedded(priv, 0x9C, 0x01);
+	bb_write_embedded(priv, 0x9C, 0);
 }
 
 /*
@@ -2269,13 +2267,13 @@ BBvSoftwareReset(struct vnt_private *priv)
  *
  */
 void
-BBvPowerSaveModeON(struct vnt_private *priv)
+bb_power_save_mode_on(struct vnt_private *priv)
 {
-	unsigned char byOrgData;
+	unsigned char by_org_data;
 
-	BBbReadEmbedded(priv, 0x0D, &byOrgData);
-	byOrgData |= BIT(0);
-	BBbWriteEmbedded(priv, 0x0D, byOrgData);
+	bb_read_embedded(priv, 0x0D, &by_org_data);
+	by_org_data |= BIT(0);
+	bb_write_embedded(priv, 0x0D, by_org_data);
 }
 
 /*
@@ -2291,13 +2289,13 @@ BBvPowerSaveModeON(struct vnt_private *priv)
  *
  */
 void
-BBvPowerSaveModeOFF(struct vnt_private *priv)
+bb_power_save_mode_off(struct vnt_private *priv)
 {
-	unsigned char byOrgData;
+	unsigned char by_org_data;
 
-	BBbReadEmbedded(priv, 0x0D, &byOrgData);
-	byOrgData &= ~(BIT(0));
-	BBbWriteEmbedded(priv, 0x0D, byOrgData);
+	bb_read_embedded(priv, 0x0D, &by_org_data);
+	by_org_data &= ~(BIT(0));
+	bb_write_embedded(priv, 0x0D, by_org_data);
 }
 
 /*
@@ -2306,7 +2304,7 @@ BBvPowerSaveModeOFF(struct vnt_private *priv)
  * Parameters:
  *  In:
  *      priv          - Device Structure
- *      byAntennaMode    - Antenna Mode
+ *      by_antenna_mode    - Antenna Mode
  *  Out:
  *      none
  *
@@ -2315,22 +2313,22 @@ BBvPowerSaveModeOFF(struct vnt_private *priv)
  */
 
 void
-BBvSetTxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
+bb_set_tx_antenna_mode(struct vnt_private *priv, unsigned char by_antenna_mode)
 {
-	unsigned char byBBTxConf;
+	unsigned char by_bb_tx_conf;
 
-	BBbReadEmbedded(priv, 0x09, &byBBTxConf); /* CR09 */
-	if (byAntennaMode == ANT_DIVERSITY) {
+	bb_read_embedded(priv, 0x09, &by_bb_tx_conf); /* CR09 */
+	if (by_antenna_mode == ANT_DIVERSITY) {
 		/* bit 1 is diversity */
-		byBBTxConf |= 0x02;
-	} else if (byAntennaMode == ANT_A) {
+		by_bb_tx_conf |= 0x02;
+	} else if (by_antenna_mode == ANT_A) {
 		/* bit 2 is ANTSEL */
-		byBBTxConf &= 0xF9; /* 1111 1001 */
-	} else if (byAntennaMode == ANT_B) {
-		byBBTxConf &= 0xFD; /* 1111 1101 */
-		byBBTxConf |= 0x04;
+		by_bb_tx_conf &= 0xF9; /* 1111 1001 */
+	} else if (by_antenna_mode == ANT_B) {
+		by_bb_tx_conf &= 0xFD; /* 1111 1101 */
+		by_bb_tx_conf |= 0x04;
 	}
-	BBbWriteEmbedded(priv, 0x09, byBBTxConf); /* CR09 */
+	bb_write_embedded(priv, 0x09, by_bb_tx_conf); /* CR09 */
 }
 
 /*
@@ -2339,7 +2337,7 @@ BBvSetTxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
  * Parameters:
  *  In:
  *      priv          - Device Structure
- *      byAntennaMode    - Antenna Mode
+ *      by_antenna_mode   - Antenna Mode
  *  Out:
  *      none
  *
@@ -2348,25 +2346,25 @@ BBvSetTxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
  */
 
 void
-BBvSetRxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
+bb_set_rx_antenna_mode(struct vnt_private *priv, unsigned char by_antenna_mode)
 {
-	unsigned char byBBRxConf;
+	unsigned char by_bb_rx_conf;
 
-	BBbReadEmbedded(priv, 0x0A, &byBBRxConf); /* CR10 */
-	if (byAntennaMode == ANT_DIVERSITY) {
-		byBBRxConf |= 0x01;
+	bb_read_embedded(priv, 0x0A, &by_bb_rx_conf); /* CR10 */
+	if (by_antenna_mode == ANT_DIVERSITY) {
+		by_bb_rx_conf |= 0x01;
 
-	} else if (byAntennaMode == ANT_A) {
-		byBBRxConf &= 0xFC; /* 1111 1100 */
-	} else if (byAntennaMode == ANT_B) {
-		byBBRxConf &= 0xFE; /* 1111 1110 */
-		byBBRxConf |= 0x02;
+	} else if (by_antenna_mode == ANT_A) {
+		by_bb_rx_conf &= 0xFC; /* 1111 1100 */
+	} else if (by_antenna_mode == ANT_B) {
+		by_bb_rx_conf &= 0xFE; /* 1111 1110 */
+		by_bb_rx_conf |= 0x02;
 	}
-	BBbWriteEmbedded(priv, 0x0A, byBBRxConf); /* CR10 */
+	bb_write_embedded(priv, 0x0A, by_bb_rx_conf); /* CR10 */
 }
 
 /*
- * Description: BBvSetDeepSleep
+ * Description: bb_set_deep_sleep
  *
  * Parameters:
  *  In:
@@ -2378,15 +2376,9 @@ BBvSetRxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode)
  *
  */
 void
-BBvSetDeepSleep(struct vnt_private *priv, unsigned char byLocalID)
+bb_set_deep_sleep(struct vnt_private *priv, unsigned char by_local_id)
 {
-	BBbWriteEmbedded(priv, 0x0C, 0x17); /* CR12 */
-	BBbWriteEmbedded(priv, 0x0D, 0xB9); /* CR13 */
+	bb_write_embedded(priv, 0x0C, 0x17); /* CR12 */
+	bb_write_embedded(priv, 0x0D, 0xB9); /* CR13 */
 }
 
-void
-BBvExitDeepSleep(struct vnt_private *priv, unsigned char byLocalID)
-{
-	BBbWriteEmbedded(priv, 0x0C, 0x00); /* CR12 */
-	BBbWriteEmbedded(priv, 0x0D, 0x01); /* CR13 */
-}
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index 0cc2e07829c5..9354ce724446 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -46,30 +46,31 @@
 #define TOP_RATE_2M         0x00200000
 #define TOP_RATE_1M         0x00100000
 
-unsigned int BBuGetFrameTime(unsigned char byPreambleType,
-			     unsigned char byPktType,
-			     unsigned int cbFrameLength,
-			     unsigned short wRate);
+unsigned int bb_get_frame_time(unsigned char by_preamble_type,
+			       unsigned char by_pkt_type,
+			       unsigned int cb_frame_length,
+			       unsigned short w_rate);
 
 void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
 		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy);
 
-bool BBbReadEmbedded(struct vnt_private *priv, unsigned char byBBAddr,
-		     unsigned char *pbyData);
-bool BBbWriteEmbedded(struct vnt_private *priv, unsigned char byBBAddr,
-		      unsigned char byData);
+bool bb_read_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		      unsigned char *pby_data);
+bool bb_write_embedded(struct vnt_private *priv, unsigned char by_bb_addr,
+		       unsigned char by_data);
 
-void BBvSetShortSlotTime(struct vnt_private *priv);
-void BBvSetVGAGainOffset(struct vnt_private *priv, unsigned char byData);
+void bb_set_short_slot_time(struct vnt_private *priv);
+void bb_set_vga_gain_offset(struct vnt_private *priv, unsigned char by_data);
 
 /* VT3253 Baseband */
-bool BBbVT3253Init(struct vnt_private *priv);
-void BBvSoftwareReset(struct vnt_private *priv);
-void BBvPowerSaveModeON(struct vnt_private *priv);
-void BBvPowerSaveModeOFF(struct vnt_private *priv);
-void BBvSetTxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode);
-void BBvSetRxAntennaMode(struct vnt_private *priv, unsigned char byAntennaMode);
-void BBvSetDeepSleep(struct vnt_private *priv, unsigned char byLocalID);
-void BBvExitDeepSleep(struct vnt_private *priv, unsigned char byLocalID);
+bool bb_vt3253_init(struct vnt_private *priv);
+void bb_software_reset(struct vnt_private *priv);
+void bb_power_save_mode_on(struct vnt_private *priv);
+void bb_power_save_mode_off(struct vnt_private *priv);
+void bb_set_tx_antenna_mode(struct vnt_private *priv,
+			    unsigned char by_antenna_mode);
+void bb_set_rx_antenna_mode(struct vnt_private *priv,
+			    unsigned char by_antenna_mode);
+void bb_set_deep_sleep(struct vnt_private *priv, unsigned char by_local_id);
 
 #endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index e65c9825ea5a..6148310c06d6 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -11,15 +11,12 @@
  *      CARDvUpdateBasicTopRate - Update BasicTopRate
  *      CARDbAddBasicRate - Add to BasicRateSet
  *      CARDbIsOFDMinBasicRate - Check if any OFDM rate is in BasicRateSet
- *      CARDvSetLoopbackMode - Set Loopback mode
- *      CARDbSoftwareReset - Sortware reset NIC
  *      CARDqGetTSFOffset - Calculate TSFOffset
  *      CARDbGetCurrentTSF - Read Current NIC TSF counter
  *      CARDqGetNextTBTT - Calculate Next Beacon TSF counter
  *      CARDvSetFirstNextTBTT - Set NIC Beacon time
  *      CARDvUpdateNextTBTT - Sync. NIC Beacon time
  *      CARDbRadioPowerOff - Turn Off NIC Radio Power
- *      CARDbRadioPowerOn - Turn On NIC Radio Power
  *
  * Revision History:
  *      06-10-2003 Bryan YC Fan:  Re-write codes to support VT3253 spec.
@@ -198,22 +195,22 @@ bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type)
 			priv->abyBBVGA[0] = 0x20;
 			priv->abyBBVGA[2] = 0x10;
 			priv->abyBBVGA[3] = 0x10;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x1C)
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
 
 		} else if (priv->byRFType == RF_UW2452) {
 			MACvSetBBType(priv->PortOffset, BB_TYPE_11A);
 			priv->abyBBVGA[0] = 0x18;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x14) {
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
-				BBbWriteEmbedded(priv, 0xE1, 0x57);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE1, 0x57);
 			}
 		} else {
 			MACvSetBBType(priv->PortOffset, BB_TYPE_11A);
 		}
-		BBbWriteEmbedded(priv, 0x88, 0x03);
+		bb_write_embedded(priv, 0x88, 0x03);
 		bySlot = C_SLOT_SHORT;
 		bySIFS = C_SIFS_A;
 		byDIFS = C_SIFS_A + 2 * C_SLOT_SHORT;
@@ -224,19 +221,19 @@ bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type)
 			priv->abyBBVGA[0] = 0x1C;
 			priv->abyBBVGA[2] = 0x00;
 			priv->abyBBVGA[3] = 0x00;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x20)
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
 
 		} else if (priv->byRFType == RF_UW2452) {
 			priv->abyBBVGA[0] = 0x14;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x18) {
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
-				BBbWriteEmbedded(priv, 0xE1, 0xD3);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE1, 0xD3);
 			}
 		}
-		BBbWriteEmbedded(priv, 0x88, 0x02);
+		bb_write_embedded(priv, 0x88, 0x02);
 		bySlot = C_SLOT_LONG;
 		bySIFS = C_SIFS_BG;
 		byDIFS = C_SIFS_BG + 2 * C_SLOT_LONG;
@@ -247,19 +244,19 @@ bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type)
 			priv->abyBBVGA[0] = 0x1C;
 			priv->abyBBVGA[2] = 0x00;
 			priv->abyBBVGA[3] = 0x00;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x20)
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
 
 		} else if (priv->byRFType == RF_UW2452) {
 			priv->abyBBVGA[0] = 0x14;
-			BBbReadEmbedded(priv, 0xE7, &byData);
+			bb_read_embedded(priv, 0xE7, &byData);
 			if (byData == 0x18) {
-				BBbWriteEmbedded(priv, 0xE7, priv->abyBBVGA[0]);
-				BBbWriteEmbedded(priv, 0xE1, 0xD3);
+				bb_write_embedded(priv, 0xE7, priv->abyBBVGA[0]);
+				bb_write_embedded(priv, 0xE1, 0xD3);
 			}
 		}
-		BBbWriteEmbedded(priv, 0x88, 0x08);
+		bb_write_embedded(priv, 0x88, 0x08);
 		bySIFS = C_SIFS_BG;
 
 		if (priv->bShortSlotTime) {
@@ -310,7 +307,7 @@ bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type)
 		priv->bySlot = bySlot;
 		VNSvOutPortB(priv->PortOffset + MAC_REG_SLOT, priv->bySlot);
 
-		BBvSetShortSlotTime(priv);
+		bb_set_short_slot_time(priv);
 	}
 	if (priv->byCWMaxMin != byCWMaxMin) {
 		priv->byCWMaxMin = byCWMaxMin;
@@ -431,7 +428,7 @@ void CARDbRadioPowerOff(struct vnt_private *priv)
 
 	MACvRegBitsOff(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
 
-	BBvSetDeepSleep(priv, priv->byLocalID);
+	bb_set_deep_sleep(priv, priv->byLocalID);
 
 	priv->bRadioOff = true;
 	pr_debug("chester power off\n");
@@ -439,60 +436,6 @@ void CARDbRadioPowerOff(struct vnt_private *priv)
 		      LED_ACTSET);  /* LED issue */
 }
 
-/*
- * Description: Turn on Radio power
- *
- * Parameters:
- *  In:
- *      priv         - The adapter to be turned on
- *  Out:
- *      none
- *
- * Return Value: true if success; otherwise false
- */
-bool CARDbRadioPowerOn(struct vnt_private *priv)
-{
-	bool bResult = true;
-
-	pr_debug("chester power on\n");
-	if (priv->bRadioControlOff) {
-		if (priv->bHWRadioOff)
-			pr_debug("chester bHWRadioOff\n");
-		if (priv->bRadioControlOff)
-			pr_debug("chester bRadioControlOff\n");
-		return false; }
-
-	if (!priv->bRadioOff) {
-		pr_debug("chester pbRadioOff\n");
-		return true; }
-
-	BBvExitDeepSleep(priv, priv->byLocalID);
-
-	MACvRegBitsOn(priv->PortOffset, MAC_REG_HOSTCR, HOSTCR_RXON);
-
-	switch (priv->byRFType) {
-	case RF_RFMD2959:
-		MACvWordRegBitsOn(priv->PortOffset, MAC_REG_SOFTPWRCTL,
-				  SOFTPWRCTL_TXPEINV);
-		MACvWordRegBitsOff(priv->PortOffset, MAC_REG_SOFTPWRCTL,
-				   SOFTPWRCTL_SWPE1);
-		break;
-
-	case RF_AIROHA:
-	case RF_AL2230S:
-	case RF_AIROHA7230:
-		MACvWordRegBitsOn(priv->PortOffset, MAC_REG_SOFTPWRCTL,
-				  (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-		break;
-	}
-
-	priv->bRadioOff = false;
-	pr_debug("chester power on\n");
-	MACvRegBitsOff(priv->PortOffset, MAC_REG_GPIOCTL0,
-		       LED_ACTSET); /* LED issue */
-	return bResult;
-}
-
 void CARDvSafeResetTx(struct vnt_private *priv)
 {
 	unsigned int uu;
@@ -816,54 +759,6 @@ unsigned char CARDbyGetPktType(struct vnt_private *priv)
 }
 
 /*
- * Description: Set NIC Loopback mode
- *
- * Parameters:
- *  In:
- *      priv         - The adapter to be set
- *      wLoopbackMode   - Loopback mode to be set
- *  Out:
- *      none
- *
- * Return Value: none
- */
-void CARDvSetLoopbackMode(struct vnt_private *priv,
-			  unsigned short wLoopbackMode)
-{
-	switch (wLoopbackMode) {
-	case CARD_LB_NONE:
-	case CARD_LB_MAC:
-	case CARD_LB_PHY:
-		break;
-	default:
-		break;
-	}
-	/* set MAC loopback */
-	MACvSetLoopbackMode(priv, LOBYTE(wLoopbackMode));
-	/* set Baseband loopback */
-}
-
-/*
- * Description: Software Reset NIC
- *
- * Parameters:
- *  In:
- *      priv         - The adapter to be reset
- *  Out:
- *      none
- *
- * Return Value: none
- */
-bool CARDbSoftwareReset(struct vnt_private *priv)
-{
-	/* reset MAC */
-	if (!MACbSafeSoftwareReset(priv))
-		return false;
-
-	return true;
-}
-
-/*
  * Description: Calculate TSF offset of two TSF input
  *              Get TSF Offset from RxBCN's TSF and local TSF
  *
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index 337266add6b2..568a2ddd6588 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -44,9 +44,6 @@ struct vnt_private;
 void CARDvSetRSPINF(struct vnt_private *priv, u8 bb_type);
 void CARDvUpdateBasicTopRate(struct vnt_private *priv);
 bool CARDbIsOFDMinBasicRate(struct vnt_private *priv);
-void CARDvSetLoopbackMode(struct vnt_private *priv,
-			  unsigned short wLoopbackMode);
-bool CARDbSoftwareReset(struct vnt_private *priv);
 void CARDvSetFirstNextTBTT(struct vnt_private *priv,
 			   unsigned short wBeaconInterval);
 void CARDvUpdateNextTBTT(struct vnt_private *priv, u64 qwTSF,
@@ -58,7 +55,6 @@ unsigned char CARDbyGetPktType(struct vnt_private *priv);
 void CARDvSafeResetTx(struct vnt_private *priv);
 void CARDvSafeResetRx(struct vnt_private *priv);
 void CARDbRadioPowerOff(struct vnt_private *priv);
-bool CARDbRadioPowerOn(struct vnt_private *priv);
 bool CARDbSetPhyParameter(struct vnt_private *priv, u8 bb_type);
 bool CARDbUpdateTSF(struct vnt_private *priv, unsigned char byRxRate,
 		    u64 qwBSSTimestamp);
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index dec6f0f23b88..62a85c1ca6c4 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -173,7 +173,7 @@ bool set_channel(struct vnt_private *priv, struct ieee80211_channel *ch)
 	    priv->byBBVGACurrent != priv->abyBBVGA[0]) {
 		priv->byBBVGACurrent = priv->abyBBVGA[0];
 
-		BBvSetVGAGainOffset(priv, priv->byBBVGACurrent);
+		bb_set_vga_gain_offset(priv, priv->byBBVGACurrent);
 	}
 
 	/* clear NAV */
@@ -195,7 +195,7 @@ bool set_channel(struct vnt_private *priv, struct ieee80211_channel *ch)
 	if (priv->bEnablePSMode)
 		RFvWriteWakeProgSyn(priv, priv->byRFType, ch->hw_value);
 
-	BBvSoftwareReset(priv);
+	bb_software_reset(priv);
 
 	if (priv->byLocalID > REV_ID_VT3253_B1) {
 		unsigned long flags;
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 5c86cc60eb5c..41cbec4134b0 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -21,18 +21,17 @@
  *   device_alloc_rx_buf - rx buffer pre-allocated function
  *   device_free_rx_buf - free rx buffer function
  *   device_free_tx_buf - free tx buffer function
- *   device_init_rd0_ring- initial rd dma0 ring
- *   device_init_rd1_ring- initial rd dma1 ring
- *   device_init_td0_ring- initial tx dma0 ring buffer
- *   device_init_td1_ring- initial tx dma1 ring buffer
- *   device_init_registers- initial MAC & BBP & RF internal registers.
- *   device_init_rings- initial tx/rx ring buffer
- *   device_free_rings- free all allocated ring buffer
- *   device_tx_srv- tx interrupt service function
+ *   device_init_rd0_ring - initial rd dma0 ring
+ *   device_init_rd1_ring - initial rd dma1 ring
+ *   device_init_td0_ring - initial tx dma0 ring buffer
+ *   device_init_td1_ring - initial tx dma1 ring buffer
+ *   device_init_registers - initial MAC & BBP & RF internal registers.
+ *   device_init_rings - initial tx/rx ring buffer
+ *   device_free_rings - free all allocated ring buffer
+ *   device_tx_srv - tx interrupt service function
  *
  * Revision History:
  */
-#undef __NO_VERSION__
 
 #include <linux/file.h>
 #include "device.h"
@@ -202,7 +201,7 @@ static void device_init_registers(struct vnt_private *priv)
 	unsigned char byOFDMPwrdBm = 0;
 
 	MACbShutdown(priv);
-	BBvSoftwareReset(priv);
+	bb_software_reset(priv);
 
 	/* Do MACbSoftwareReset in MACvInitialize */
 	MACbSoftwareReset(priv);
@@ -279,8 +278,8 @@ static void device_init_registers(struct vnt_private *priv)
 	}
 
 	/* Set initial antenna mode */
-	BBvSetTxAntennaMode(priv, priv->byTxAntennaMode);
-	BBvSetRxAntennaMode(priv, priv->byRxAntennaMode);
+	bb_set_tx_antenna_mode(priv, priv->byTxAntennaMode);
+	bb_set_rx_antenna_mode(priv, priv->byRxAntennaMode);
 
 	/* zonetype initial */
 	priv->byOriginalZonetype = priv->abyEEPROM[EEP_OFS_ZONETYPE];
@@ -357,16 +356,16 @@ static void device_init_registers(struct vnt_private *priv)
 	VNSvOutPortB(priv->PortOffset + MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
 
 	/* initialize BBP registers */
-	BBbVT3253Init(priv);
+	bb_vt3253_init(priv);
 
 	if (priv->bUpdateBBVGA) {
 		priv->byBBVGACurrent = priv->abyBBVGA[0];
 		priv->byBBVGANew = priv->byBBVGACurrent;
-		BBvSetVGAGainOffset(priv, priv->abyBBVGA[0]);
+		bb_set_vga_gain_offset(priv, priv->abyBBVGA[0]);
 	}
 
-	BBvSetRxAntennaMode(priv, priv->byRxAntennaMode);
-	BBvSetTxAntennaMode(priv, priv->byTxAntennaMode);
+	bb_set_rx_antenna_mode(priv, priv->byRxAntennaMode);
+	bb_set_tx_antenna_mode(priv, priv->byTxAntennaMode);
 
 	/* Set BB and packet type at the same time. */
 	/* Set Short Slot Time, xIFS, and RSPINF. */
@@ -1001,7 +1000,7 @@ static void vnt_check_bb_vga(struct vnt_private *priv)
 
 	if (priv->uBBVGADiffCount == 1) {
 		/* first VGA diff gain */
-		BBvSetVGAGainOffset(priv, priv->byBBVGANew);
+		bb_set_vga_gain_offset(priv, priv->byBBVGANew);
 
 		dev_dbg(&priv->pcid->dev,
 			"First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
@@ -1017,7 +1016,7 @@ static void vnt_check_bb_vga(struct vnt_private *priv)
 			priv->byBBVGACurrent,
 			(int)priv->uBBVGADiffCount);
 
-		BBvSetVGAGainOffset(priv, priv->byBBVGANew);
+		bb_set_vga_gain_offset(priv, priv->byBBVGANew);
 	}
 }
 
@@ -1445,7 +1444,7 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 			priv->bShortSlotTime = false;
 
 		CARDbSetPhyParameter(priv, priv->byBBType);
-		BBvSetVGAGainOffset(priv, priv->abyBBVGA[0]);
+		bb_set_vga_gain_offset(priv, priv->abyBBVGA[0]);
 	}
 
 	if (changed & BSS_CHANGED_TXPOWER)
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index d6ca6e5551a7..747d79265a7c 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -419,7 +419,7 @@ static bool s_bAL7230Init(struct vnt_private *priv)
 
 	MACvWordRegBitsOn(iobase, MAC_REG_SOFTPWRCTL, (SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
-	BBvPowerSaveModeOFF(priv); /* RobertYu:20050106, have DC value for Calibration */
+	bb_power_save_mode_off(priv); /* RobertYu:20050106, have DC value for Calibration */
 
 	for (ii = 0; ii < CB_AL7230_INIT_SEQ; ii++)
 		ret &= IFRFbWriteEmbedded(priv, dwAL7230InitTable[ii]);
@@ -443,7 +443,7 @@ static bool s_bAL7230Init(struct vnt_private *priv)
 							 SOFTPWRCTL_SWPECTI  |
 							 SOFTPWRCTL_TXPEINV));
 
-	BBvPowerSaveModeON(priv); /* RobertYu:20050106 */
+	bb_power_save_mode_on(priv); /* RobertYu:20050106 */
 
 	/* PE1: TX_ON, PE2: RX_ON, PE3: PLLON */
 	/* 3-wire control for power saving mode */
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 37fcc42ed000..cfab64d2b312 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -165,16 +165,21 @@ s_uGetTxRsvTime(
 {
 	unsigned int uDataTime, uAckTime;
 
-	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
-	if (byPktType == PK_TYPE_11B) /* llb,CCK mode */
-		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopCCKBasicRate);
-	else /* 11g 2.4G OFDM mode & 11a 5G OFDM mode */
-		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, (unsigned short)pDevice->byTopOFDMBasicRate);
-
-	if (bNeedAck)
-		return uDataTime + pDevice->uSIFS + uAckTime;
-	else
+	uDataTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, cbFrameLength, wRate);
+
+	if (!bNeedAck)
 		return uDataTime;
+
+	/*
+	 * CCK mode  - 11b
+	 * OFDM mode - 11g 2.4G & 11a 5G
+	 */
+	uAckTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14,
+				     byPktType == PK_TYPE_11B ?
+				     pDevice->byTopCCKBasicRate :
+				     pDevice->byTopOFDMBasicRate);
+
+	return uDataTime + pDevice->uSIFS + uAckTime;
 }
 
 static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
@@ -195,24 +200,28 @@ s_uGetRTSCTSRsvTime(
 	unsigned short wCurrentRate
 )
 {
-	unsigned int uRrvTime, uRTSTime, uCTSTime, uAckTime, uDataTime;
-
-	uRrvTime = uRTSTime = uCTSTime = uAckTime = uDataTime = 0;
+	unsigned int uRrvTime = 0;
+	unsigned int uRTSTime = 0;
+	unsigned int uCTSTime = 0;
+	unsigned int uAckTime = 0;
+	unsigned int uDataTime = 0;
 
-	uDataTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
+	uDataTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, cbFrameLength, wCurrentRate);
 	if (byRTSRsvType == 0) { /* RTSTxRrvTime_bb */
-		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
-		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uRTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
+		uAckTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uCTSTime = uAckTime;
 	} else if (byRTSRsvType == 1) { /* RTSTxRrvTime_ba, only in 2.4GHZ */
-		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uRTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 20, pDevice->byTopCCKBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uAckTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 	} else if (byRTSRsvType == 2) { /* RTSTxRrvTime_aa */
-		uRTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 20, pDevice->byTopOFDMBasicRate);
-		uCTSTime = uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uRTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 20, pDevice->byTopOFDMBasicRate);
+		uAckTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uCTSTime = uAckTime;
 	} else if (byRTSRsvType == 3) { /* CTSTxRrvTime_ba, only in 2.4GHZ */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uAckTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		uRrvTime = uCTSTime + uAckTime + uDataTime + 2 * pDevice->uSIFS;
 		return cpu_to_le16((u16)uRrvTime);
 	}
@@ -239,138 +248,83 @@ s_uGetDataDuration(
 )
 {
 	bool bLastFrag = false;
-	unsigned int uAckTime = 0, uNextPktTime = 0;
+	unsigned int uAckTime = 0, uNextPktTime = 0, len;
 
 	if (uFragIdx == (uMACfragNum - 1))
 		bLastFrag = true;
 
+	if (uFragIdx == (uMACfragNum - 2))
+		len = cbLastFragmentSize;
+	else
+		len = cbFrameLength;
+
 	switch (byDurType) {
 	case DATADUR_B:    /* DATADUR_B */
-		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-				return pDevice->uSIFS + uAckTime;
-			} else {
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->byPreambleType,
+						     byPktType, 14,
+						     pDevice->byTopCCKBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
 				return 0;
-			}
-		} else {/* First Frag or Mid Frag */
-			if (uFragIdx == (uMACfragNum - 2))
-				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			else
-				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
-				return pDevice->uSIFS + uAckTime + uNextPktTime;
-			} else {
-				return pDevice->uSIFS + uNextPktTime;
-			}
+		} else {
+			/* First Frag or Mid Frag */
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
 		}
-		break;
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
 
 	case DATADUR_A:    /* DATADUR_A */
-		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime;
-			} else {
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->byPreambleType,
+						     byPktType, 14,
+						     pDevice->byTopOFDMBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
 				return 0;
-			}
-		} else {/* First Frag or Mid Frag */
-			if (uFragIdx == (uMACfragNum - 2))
-				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wRate, bNeedAck);
-			else
-				uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
-
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime + uNextPktTime;
-			} else {
-				return pDevice->uSIFS + uNextPktTime;
-			}
+		} else {
+			/* First Frag or Mid Frag */
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
 		}
-		break;
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
 
 	case DATADUR_A_F0:    /* DATADUR_A_F0 */
-		if (((uMACfragNum == 1)) || bLastFrag) {/* Non Frag or Last Frag */
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime;
-			} else {
+	case DATADUR_A_F1:    /* DATADUR_A_F1 */
+		if (bNeedAck) {
+			uAckTime = bb_get_frame_time(pDevice->byPreambleType,
+						     byPktType, 14,
+						     pDevice->byTopOFDMBasicRate);
+		}
+		/* Non Frag or Last Frag */
+		if ((uMACfragNum == 1) || bLastFrag) {
+			if (!bNeedAck)
 				return 0;
-			}
-		} else { /* First Frag or Mid Frag */
-			if (byFBOption == AUTO_FB_0) {
-				if (wRate < RATE_18M)
-					wRate = RATE_18M;
-				else if (wRate > RATE_54M)
-					wRate = RATE_54M;
-
-				if (uFragIdx == (uMACfragNum - 2))
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				else
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
-
-			} else { /* (byFBOption == AUTO_FB_1) */
-				if (wRate < RATE_18M)
-					wRate = RATE_18M;
-				else if (wRate > RATE_54M)
-					wRate = RATE_54M;
-
-				if (uFragIdx == (uMACfragNum - 2))
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-				else
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE0][wRate-RATE_18M], bNeedAck);
-			}
+		} else {
+			/* First Frag or Mid Frag */
+			if (wRate < RATE_18M)
+				wRate = RATE_18M;
+			else if (wRate > RATE_54M)
+				wRate = RATE_54M;
 
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime + uNextPktTime;
-			} else {
-				return pDevice->uSIFS + uNextPktTime;
-			}
-		}
-		break;
+			wRate -= RATE_18M;
 
-	case DATADUR_A_F1:    /* DATADUR_A_F1 */
-		if (((uMACfragNum == 1)) || bLastFrag) { /* Non Frag or Last Frag */
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime;
-			} else {
-				return 0;
-			}
-		} else { /* First Frag or Mid Frag */
-			if (byFBOption == AUTO_FB_0) {
-				if (wRate < RATE_18M)
-					wRate = RATE_18M;
-				else if (wRate > RATE_54M)
-					wRate = RATE_54M;
-
-				if (uFragIdx == (uMACfragNum - 2))
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				else
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
-
-			} else { /* (byFBOption == AUTO_FB_1) */
-				if (wRate < RATE_18M)
-					wRate = RATE_18M;
-				else if (wRate > RATE_54M)
-					wRate = RATE_54M;
-
-				if (uFragIdx == (uMACfragNum - 2))
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbLastFragmentSize, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-				else
-					uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt1[FB_RATE1][wRate-RATE_18M], bNeedAck);
-			}
-			if (bNeedAck) {
-				uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
-				return pDevice->uSIFS + uAckTime + uNextPktTime;
-			} else {
-				return pDevice->uSIFS + uNextPktTime;
-			}
+			if (byFBOption == AUTO_FB_0)
+				wRate = wFB_Opt0[FB_RATE0][wRate];
+			else
+				wRate = wFB_Opt1[FB_RATE0][wRate];
+
+			uNextPktTime = s_uGetTxRsvTime(pDevice, byPktType,
+						       len, wRate, bNeedAck);
 		}
-		break;
+
+		return pDevice->uSIFS + uAckTime + uNextPktTime;
 
 	default:
 		break;
@@ -396,17 +350,17 @@ s_uGetRTSCTSDuration(
 
 	switch (byDurType) {
 	case RTSDUR_BB:    /* RTSDuration_bb */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
 	case RTSDUR_BA:    /* RTSDuration_ba */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
 	case RTSDUR_AA:    /* RTSDuration_aa */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wRate, bNeedAck);
 		break;
 
@@ -415,7 +369,7 @@ s_uGetRTSCTSDuration(
 		break;
 
 	case RTSDUR_BA_F0: /* RTSDuration_ba_f0 */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -424,7 +378,7 @@ s_uGetRTSCTSDuration(
 		break;
 
 	case RTSDUR_AA_F0: /* RTSDuration_aa_f0 */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE0][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -433,7 +387,7 @@ s_uGetRTSCTSDuration(
 		break;
 
 	case RTSDUR_BA_F1: /* RTSDuration_ba_f1 */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2*pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -442,7 +396,7 @@ s_uGetRTSCTSDuration(
 		break;
 
 	case RTSDUR_AA_F1: /* RTSDuration_aa_f1 */
-		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
+		uCTSTime = bb_get_frame_time(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		if ((byFBOption == AUTO_FB_0) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
 			uDurTime = uCTSTime + 2 * pDevice->uSIFS + s_uGetTxRsvTime(pDevice, byPktType, cbFrameLength, wFB_Opt0[FB_RATE1][wRate-RATE_18M], bNeedAck);
 		else if ((byFBOption == AUTO_FB_1) && (wRate >= RATE_18M) && (wRate <= RATE_54M))
@@ -1040,16 +994,14 @@ s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
 	bool bRTS = (bool)(fifo_ctl & FIFOCTL_RTS);
 	struct vnt_tx_desc *ptdCurr;
 	unsigned int cbHeaderLength = 0;
-	void *pvRrvTime;
-	struct vnt_mic_hdr *pMICHDR;
-	void *pvRTS;
-	void *pvCTS;
-	void *pvTxDataHd;
+	void *pvRrvTime = NULL;
+	struct vnt_mic_hdr *pMICHDR = NULL;
+	void *pvRTS = NULL;
+	void *pvCTS = NULL;
+	void *pvTxDataHd = NULL;
 	unsigned short wTxBufSize;   /* FFinfo size */
 	unsigned char byFBOption = AUTO_FB_NONE;
 
-	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
-
 	cbFrameSize = skb->len + 4;
 
 	if (info->control.hw_key) {
diff --git a/drivers/staging/vt6656/Makefile b/drivers/staging/vt6656/Makefile
index 375f54e9f58b..f696a9d7a143 100644
--- a/drivers/staging/vt6656/Makefile
+++ b/drivers/staging/vt6656/Makefile
@@ -1,7 +1,4 @@
 # SPDX-License-Identifier: GPL-2.0
-# TODO: all of these should be removed
-ccflags-y := -DLINUX -D__KERNEL__ -DEXPORT_SYMTAB -D__NO_VERSION__
-ccflags-y += -DHOSTAP
 
 vt6656_stage-y +=	main_usb.o \
 			card.o  \
@@ -13,7 +10,6 @@ vt6656_stage-y +=	main_usb.o \
 			key.o \
 			rf.o \
 			usbpipe.o \
-			channel.o \
-			firmware.o
+			channel.o
 
 obj-$(CONFIG_VT6656) +=	vt6656_stage.o
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index a19a563d8bcc..41ae779ec61f 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -23,13 +23,15 @@
  */
 
 #include <linux/bits.h>
+#include <linux/errno.h>
 #include <linux/kernel.h>
+#include "device.h"
 #include "mac.h"
 #include "baseband.h"
 #include "rf.h"
 #include "usbpipe.h"
 
-static u8 vnt_vt3184_agc[] = {
+static const u8 vnt_vt3184_agc[] = {
 	0x00, 0x00, 0x02, 0x02, 0x04, 0x04, 0x06, 0x06,
 	0x08, 0x08, 0x0a, 0x0a, 0x0c, 0x0c, 0x0e, 0x0e, /* 0x0f */
 	0x10, 0x10, 0x12, 0x12, 0x14, 0x14, 0x16, 0x16,
@@ -76,7 +78,7 @@ static u8 vnt_vt3184_al2230[] = {
 };
 
 /* {{RobertYu:20060515, new BB setting for VT3226D0 */
-static u8 vnt_vt3184_vt3226d0[] = {
+static const u8 vnt_vt3184_vt3226d0[] = {
 	0x31, 0x00, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00,
 	0x70, 0x45, 0x2a, 0x76, 0x00, 0x00, 0x80, 0x00, /* 0x0f */
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
@@ -111,198 +113,85 @@ static u8 vnt_vt3184_vt3226d0[] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00  /* 0xff */
 };
 
-static const u16 vnt_frame_time[MAX_RATE] = {
-	10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216
+struct vnt_threshold {
+	u8 bb_pre_ed_rssi;
+	u8 cr_201;
+	u8 cr_206;
 };
 
-/*
- * Description: Calculate data frame transmitting time
- *
- * Parameters:
- *  In:
- *	preamble_type	- Preamble Type
- *	pkt_type	- PK_TYPE_11A, PK_TYPE_11B, PK_TYPE_11GB, PK_TYPE_11GA
- *	frame_length	- Baseband Type
- *	tx_rate		- Tx Rate
- *  Out:
- *
- * Return Value: FrameTime
- *
- */
-unsigned int vnt_get_frame_time(u8 preamble_type, u8 pkt_type,
-				unsigned int frame_length, u16 tx_rate)
-{
-	unsigned int frame_time;
-	unsigned int preamble;
-	unsigned int rate = 0;
-
-	if (tx_rate > RATE_54M)
-		return 0;
-
-	rate = (unsigned int)vnt_frame_time[tx_rate];
-
-	if (tx_rate <= 3) {
-		if (preamble_type == 1)
-			preamble = 96;
-		else
-			preamble = 192;
-
-		frame_time = DIV_ROUND_UP(frame_length * 80, rate);
-		return preamble + frame_time;
-	}
-
-	frame_time = DIV_ROUND_UP(frame_length * 8 + 22, rate);
-	frame_time = frame_time * 4;
-
-	if (pkt_type != PK_TYPE_11A)
-		frame_time += 6;
-	return 20 + frame_time;
-}
-
-/*
- * Description: Calculate Length, Service, and Signal fields of Phy for Tx
- *
- * Parameters:
- *  In:
- *      priv         - Device Structure
- *      frame_length   - Tx Frame Length
- *      tx_rate           - Tx Rate
- *  Out:
- *	struct vnt_phy_field *phy
- *		- pointer to Phy Length field
- *		- pointer to Phy Service field
- *		- pointer to Phy Signal field
- *
- * Return Value: none
- *
- */
-void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
-		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy)
-{
-	u32 bit_count;
-	u32 count = 0;
-	u32 tmp;
-	int ext_bit;
-	u8 preamble_type = priv->preamble_type;
-
-	bit_count = frame_length * 8;
-	ext_bit = false;
-
-	switch (tx_rate) {
-	case RATE_1M:
-		count = bit_count;
-
-		phy->signal = 0x00;
-
-		break;
-	case RATE_2M:
-		count = bit_count / 2;
-
-		if (preamble_type == 1)
-			phy->signal = 0x09;
-		else
-			phy->signal = 0x01;
-
-		break;
-	case RATE_5M:
-		count = DIV_ROUND_UP(bit_count * 10, 55);
-
-		if (preamble_type == 1)
-			phy->signal = 0x0a;
-		else
-			phy->signal = 0x02;
-
-		break;
-	case RATE_11M:
-		count = bit_count / 11;
-		tmp = count * 11;
-
-		if (tmp != bit_count) {
-			count++;
-
-			if ((bit_count - tmp) <= 3)
-				ext_bit = true;
-		}
-
-		if (preamble_type == 1)
-			phy->signal = 0x0b;
-		else
-			phy->signal = 0x03;
-
-		break;
-	case RATE_6M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9b;
-		else
-			phy->signal = 0x8b;
-
-		break;
-	case RATE_9M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9f;
-		else
-			phy->signal = 0x8f;
-
-		break;
-	case RATE_12M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9a;
-		else
-			phy->signal = 0x8a;
-
-		break;
-	case RATE_18M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9e;
-		else
-			phy->signal = 0x8e;
-
-		break;
-	case RATE_24M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x99;
-		else
-			phy->signal = 0x89;
-
-		break;
-	case RATE_36M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9d;
-		else
-			phy->signal = 0x8d;
-
-		break;
-	case RATE_48M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x98;
-		else
-			phy->signal = 0x88;
+static const struct vnt_threshold al2230_vnt_threshold[] = {
+	{0, 0x00, 0x30},	/* Max sensitivity */
+	{68, 0x00, 0x36},
+	{67, 0x00, 0x43},
+	{66, 0x00, 0x51},
+	{65, 0x00, 0x62},
+	{64, 0x00, 0x79},
+	{63, 0x00, 0x93},
+	{62, 0x00, 0xb9},
+	{61, 0x00, 0xe3},
+	{60, 0x01, 0x18},
+	{59, 0x01, 0x54},
+	{58, 0x01, 0xa0},
+	{57, 0x02, 0x20},
+	{56, 0x02, 0xa0},
+	{55, 0x03, 0x00},
+	{53, 0x06, 0x00},
+	{51, 0x09, 0x00},
+	{49, 0x0e, 0x00},
+	{47, 0x15, 0x00},
+	{46, 0x1a, 0x00},
+	{45, 0xff, 0x00}
+};
 
-		break;
-	case RATE_54M:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9c;
-		else
-			phy->signal = 0x8c;
-		break;
-	default:
-		if (pkt_type == PK_TYPE_11A)
-			phy->signal = 0x9c;
-		else
-			phy->signal = 0x8c;
-		break;
-	}
+static const struct vnt_threshold vt3226_vnt_threshold[] = {
+	{0, 0x00, 0x24},	/* Max sensitivity */
+	{68, 0x00, 0x2d},
+	{67, 0x00, 0x36},
+	{66, 0x00, 0x43},
+	{65, 0x00, 0x52},
+	{64, 0x00, 0x68},
+	{63, 0x00, 0x80},
+	{62, 0x00, 0x9c},
+	{61, 0x00, 0xc0},
+	{60, 0x00, 0xea},
+	{59, 0x01, 0x30},
+	{58, 0x01, 0x70},
+	{57, 0x01, 0xb0},
+	{56, 0x02, 0x30},
+	{55, 0x02, 0xc0},
+	{53, 0x04, 0x00},
+	{51, 0x07, 0x00},
+	{49, 0x0a, 0x00},
+	{47, 0x11, 0x00},
+	{45, 0x18, 0x00},
+	{43, 0x26, 0x00},
+	{42, 0x36, 0x00},
+	{41, 0xff, 0x00}
+};
 
-	if (pkt_type == PK_TYPE_11B) {
-		phy->service = 0x00;
-		if (ext_bit)
-			phy->service |= 0x80;
-		phy->len = cpu_to_le16((u16)count);
-	} else {
-		phy->service = 0x00;
-		phy->len = cpu_to_le16((u16)frame_length);
-	}
-}
+static const struct vnt_threshold vt3342_vnt_threshold[] = {
+	{0, 0x00, 0x38},	/* Max sensitivity */
+	{66, 0x00, 0x43},
+	{65, 0x00, 0x52},
+	{64, 0x00, 0x68},
+	{63, 0x00, 0x80},
+	{62, 0x00, 0x9c},
+	{61, 0x00, 0xc0},
+	{60, 0x00, 0xea},
+	{59, 0x01, 0x30},
+	{58, 0x01, 0x70},
+	{57, 0x01, 0xb0},
+	{56, 0x02, 0x30},
+	{55, 0x02, 0xc0},
+	{53, 0x04, 0x00},
+	{51, 0x07, 0x00},
+	{49, 0x0a, 0x00},
+	{47, 0x11, 0x00},
+	{45, 0x18, 0x00},
+	{43, 0x26, 0x00},
+	{42, 0x36, 0x00},
+	{41, 0xff, 0x00}
+};
 
 /*
  * Description: Set Antenna mode
@@ -352,9 +241,10 @@ int vnt_set_antenna_mode(struct vnt_private *priv, u8 antenna_mode)
 
 int vnt_vt3184_init(struct vnt_private *priv)
 {
-	int ret = 0;
+	int ret;
 	u16 length;
-	u8 *addr;
+	u8 *addr = NULL;
+	const u8 *c_addr;
 	u8 data;
 
 	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, 0, MESSAGE_REQUEST_EEPROM,
@@ -366,23 +256,15 @@ int vnt_vt3184_init(struct vnt_private *priv)
 
 	dev_dbg(&priv->usb->dev, "RF Type %d\n", priv->rf_type);
 
-	if (priv->rf_type == RF_AL2230 ||
-	    priv->rf_type == RF_AL2230S) {
+	if ((priv->rf_type == RF_AL2230) ||
+	    (priv->rf_type == RF_AL2230S) ||
+	    (priv->rf_type == RF_AIROHA7230)) {
 		priv->bb_rx_conf = vnt_vt3184_al2230[10];
 		length = sizeof(vnt_vt3184_al2230);
 		addr = vnt_vt3184_al2230;
 
-		priv->bb_vga[0] = 0x1C;
-		priv->bb_vga[1] = 0x10;
-		priv->bb_vga[2] = 0x0;
-		priv->bb_vga[3] = 0x0;
-
-	} else if (priv->rf_type == RF_AIROHA7230) {
-		priv->bb_rx_conf = vnt_vt3184_al2230[10];
-		length = sizeof(vnt_vt3184_al2230);
-		addr = vnt_vt3184_al2230;
-
-		addr[0xd7] = 0x06;
+		if (priv->rf_type == RF_AIROHA7230)
+			addr[0xd7] = 0x06;
 
 		priv->bb_vga[0] = 0x1c;
 		priv->bb_vga[1] = 0x10;
@@ -390,25 +272,11 @@ int vnt_vt3184_init(struct vnt_private *priv)
 		priv->bb_vga[3] = 0x0;
 
 	} else if ((priv->rf_type == RF_VT3226) ||
-			(priv->rf_type == RF_VT3226D0)) {
+		   (priv->rf_type == RF_VT3226D0) ||
+		   (priv->rf_type == RF_VT3342A0)) {
 		priv->bb_rx_conf = vnt_vt3184_vt3226d0[10];
 		length = sizeof(vnt_vt3184_vt3226d0);
-		addr = vnt_vt3184_vt3226d0;
-
-		priv->bb_vga[0] = 0x20;
-		priv->bb_vga[1] = 0x10;
-		priv->bb_vga[2] = 0x0;
-		priv->bb_vga[3] = 0x0;
-
-		/* Fix VT3226 DFC system timing issue */
-		ret = vnt_mac_reg_bits_on(priv, MAC_REG_SOFTPWRCTL2,
-					  SOFTPWRCTL_RFLEOPT);
-		if (ret)
-			goto end;
-	} else if (priv->rf_type == RF_VT3342A0) {
-		priv->bb_rx_conf = vnt_vt3184_vt3226d0[10];
-		length = sizeof(vnt_vt3184_vt3226d0);
-		addr = vnt_vt3184_vt3226d0;
+		c_addr = vnt_vt3184_vt3226d0;
 
 		priv->bb_vga[0] = 0x20;
 		priv->bb_vga[1] = 0x10;
@@ -424,8 +292,11 @@ int vnt_vt3184_init(struct vnt_private *priv)
 		goto end;
 	}
 
+	if (addr)
+		c_addr = addr;
+
 	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
-				     MESSAGE_REQUEST_BBREG, length, addr);
+				     MESSAGE_REQUEST_BBREG, length, c_addr);
 	if (ret)
 		goto end;
 
@@ -435,19 +306,13 @@ int vnt_vt3184_init(struct vnt_private *priv)
 	if (ret)
 		goto end;
 
-	if (priv->rf_type == RF_VT3226 ||
-	    priv->rf_type == RF_VT3342A0) {
-		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
-					 MAC_REG_ITRTMSET, 0x23);
-		if (ret)
-			goto end;
+	if ((priv->rf_type == RF_VT3226) ||
+	    (priv->rf_type == RF_VT3342A0) ||
+	    (priv->rf_type == RF_VT3226D0)) {
+		data = (priv->rf_type == RF_VT3226D0) ? 0x11 : 0x23;
 
-		ret = vnt_mac_reg_bits_on(priv, MAC_REG_PAPEDELAY, BIT(0));
-		if (ret)
-			goto end;
-	} else if (priv->rf_type == RF_VT3226D0) {
 		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG,
-					 MAC_REG_ITRTMSET, 0x11);
+					 MAC_REG_ITRTMSET, data);
 		if (ret)
 			goto end;
 
@@ -507,21 +372,22 @@ int vnt_set_short_slot_time(struct vnt_private *priv)
 
 	ret = vnt_control_in_u8(priv, MESSAGE_REQUEST_BBREG, 0xe7, &bb_vga);
 	if (ret)
-		goto end;
+		return ret;
 
 	if (bb_vga == priv->bb_vga[0])
 		priv->bb_rx_conf |= 0x20;
 
-	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a,
-				 priv->bb_rx_conf);
-
-end:
-	return ret;
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a,
+				  priv->bb_rx_conf);
 }
 
-void vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data)
+int vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data)
 {
-	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xE7, data);
+	int ret;
+
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xE7, data);
+	if (ret)
+		return ret;
 
 	/* patch for 3253B0 Baseband with Cardbus module */
 	if (priv->short_slot_time)
@@ -529,7 +395,8 @@ void vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data)
 	else
 		priv->bb_rx_conf |= 0x20; /* 0010 0000 */
 
-	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a, priv->bb_rx_conf);
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0a,
+				  priv->bb_rx_conf);
 }
 
 /*
@@ -570,268 +437,57 @@ int vnt_exit_deep_sleep(struct vnt_private *priv)
 	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x0d, 0x01);
 }
 
-void vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning)
+int vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning)
 {
-	u8 cr_201 = 0x0, cr_206 = 0x0;
-	u8 ed_inx = priv->bb_pre_ed_index;
+	const struct vnt_threshold *threshold = NULL;
+	u8 length;
+	u8 cr_201, cr_206;
+	u8 ed_inx;
+	int ret;
 
 	switch (priv->rf_type) {
 	case RF_AL2230:
 	case RF_AL2230S:
 	case RF_AIROHA7230:
-		if (scanning) { /* Max sensitivity */
-			ed_inx = 0;
-			cr_206 = 0x30;
-			break;
-		}
-
-		if (priv->bb_pre_ed_rssi <= 45) {
-			ed_inx = 20;
-			cr_201 = 0xff;
-		} else if (priv->bb_pre_ed_rssi <= 46) {
-			ed_inx = 19;
-			cr_201 = 0x1a;
-		} else if (priv->bb_pre_ed_rssi <= 47) {
-			ed_inx = 18;
-			cr_201 = 0x15;
-		} else if (priv->bb_pre_ed_rssi <= 49) {
-			ed_inx = 17;
-			cr_201 = 0xe;
-		} else if (priv->bb_pre_ed_rssi <= 51) {
-			ed_inx = 16;
-			cr_201 = 0x9;
-		} else if (priv->bb_pre_ed_rssi <= 53) {
-			ed_inx = 15;
-			cr_201 = 0x6;
-		} else if (priv->bb_pre_ed_rssi <= 55) {
-			ed_inx = 14;
-			cr_201 = 0x3;
-		} else if (priv->bb_pre_ed_rssi <= 56) {
-			ed_inx = 13;
-			cr_201 = 0x2;
-			cr_206 = 0xa0;
-		} else if (priv->bb_pre_ed_rssi <= 57) {
-			ed_inx = 12;
-			cr_201 = 0x2;
-			cr_206 = 0x20;
-		} else if (priv->bb_pre_ed_rssi <= 58) {
-			ed_inx = 11;
-			cr_201 = 0x1;
-			cr_206 = 0xa0;
-		} else if (priv->bb_pre_ed_rssi <= 59) {
-			ed_inx = 10;
-			cr_201 = 0x1;
-			cr_206 = 0x54;
-		} else if (priv->bb_pre_ed_rssi <= 60) {
-			ed_inx = 9;
-			cr_201 = 0x1;
-			cr_206 = 0x18;
-		} else if (priv->bb_pre_ed_rssi <= 61) {
-			ed_inx = 8;
-			cr_206 = 0xe3;
-		} else if (priv->bb_pre_ed_rssi <= 62) {
-			ed_inx = 7;
-			cr_206 = 0xb9;
-		} else if (priv->bb_pre_ed_rssi <= 63) {
-			ed_inx = 6;
-			cr_206 = 0x93;
-		} else if (priv->bb_pre_ed_rssi <= 64) {
-			ed_inx = 5;
-			cr_206 = 0x79;
-		} else if (priv->bb_pre_ed_rssi <= 65) {
-			ed_inx = 4;
-			cr_206 = 0x62;
-		} else if (priv->bb_pre_ed_rssi <= 66) {
-			ed_inx = 3;
-			cr_206 = 0x51;
-		} else if (priv->bb_pre_ed_rssi <= 67) {
-			ed_inx = 2;
-			cr_206 = 0x43;
-		} else if (priv->bb_pre_ed_rssi <= 68) {
-			ed_inx = 1;
-			cr_206 = 0x36;
-		} else {
-			ed_inx = 0;
-			cr_206 = 0x30;
-		}
+		threshold = al2230_vnt_threshold;
+		length = ARRAY_SIZE(al2230_vnt_threshold);
 		break;
 
 	case RF_VT3226:
 	case RF_VT3226D0:
-		if (scanning)	{ /* Max sensitivity */
-			ed_inx = 0;
-			cr_206 = 0x24;
-			break;
-		}
-
-		if (priv->bb_pre_ed_rssi <= 41) {
-			ed_inx = 22;
-			cr_201 = 0xff;
-		} else if (priv->bb_pre_ed_rssi <= 42) {
-			ed_inx = 21;
-			cr_201 = 0x36;
-		} else if (priv->bb_pre_ed_rssi <= 43) {
-			ed_inx = 20;
-			cr_201 = 0x26;
-		} else if (priv->bb_pre_ed_rssi <= 45) {
-			ed_inx = 19;
-			cr_201 = 0x18;
-		} else if (priv->bb_pre_ed_rssi <= 47) {
-			ed_inx = 18;
-			cr_201 = 0x11;
-		} else if (priv->bb_pre_ed_rssi <= 49) {
-			ed_inx = 17;
-			cr_201 = 0xa;
-		} else if (priv->bb_pre_ed_rssi <= 51) {
-			ed_inx = 16;
-			cr_201 = 0x7;
-		} else if (priv->bb_pre_ed_rssi <= 53) {
-			ed_inx = 15;
-			cr_201 = 0x4;
-		} else if (priv->bb_pre_ed_rssi <= 55) {
-			ed_inx = 14;
-			cr_201 = 0x2;
-			cr_206 = 0xc0;
-		} else if (priv->bb_pre_ed_rssi <= 56) {
-			ed_inx = 13;
-			cr_201 = 0x2;
-			cr_206 = 0x30;
-		} else if (priv->bb_pre_ed_rssi <= 57) {
-			ed_inx = 12;
-			cr_201 = 0x1;
-			cr_206 = 0xb0;
-		} else if (priv->bb_pre_ed_rssi <= 58) {
-			ed_inx = 11;
-			cr_201 = 0x1;
-			cr_206 = 0x70;
-		} else if (priv->bb_pre_ed_rssi <= 59) {
-			ed_inx = 10;
-			cr_201 = 0x1;
-			cr_206 = 0x30;
-		} else if (priv->bb_pre_ed_rssi <= 60) {
-			ed_inx = 9;
-			cr_206 = 0xea;
-		} else if (priv->bb_pre_ed_rssi <= 61) {
-			ed_inx = 8;
-			cr_206 = 0xc0;
-		} else if (priv->bb_pre_ed_rssi <= 62) {
-			ed_inx = 7;
-			cr_206 = 0x9c;
-		} else if (priv->bb_pre_ed_rssi <= 63) {
-			ed_inx = 6;
-			cr_206 = 0x80;
-		} else if (priv->bb_pre_ed_rssi <= 64) {
-			ed_inx = 5;
-			cr_206 = 0x68;
-		} else if (priv->bb_pre_ed_rssi <= 65) {
-			ed_inx = 4;
-			cr_206 = 0x52;
-		} else if (priv->bb_pre_ed_rssi <= 66) {
-			ed_inx = 3;
-			cr_206 = 0x43;
-		} else if (priv->bb_pre_ed_rssi <= 67) {
-			ed_inx = 2;
-			cr_206 = 0x36;
-		} else if (priv->bb_pre_ed_rssi <= 68) {
-			ed_inx = 1;
-			cr_206 = 0x2d;
-		} else {
-			ed_inx = 0;
-			cr_206 = 0x24;
-		}
+		threshold = vt3226_vnt_threshold;
+		length = ARRAY_SIZE(vt3226_vnt_threshold);
 		break;
 
 	case RF_VT3342A0:
-		if (scanning) { /* need Max sensitivity */
-			ed_inx = 0;
-			cr_206 = 0x38;
-			break;
-		}
-
-		if (priv->bb_pre_ed_rssi <= 41) {
-			ed_inx = 20;
-			cr_201 = 0xff;
-		} else if (priv->bb_pre_ed_rssi <= 42) {
-			ed_inx = 19;
-			cr_201 = 0x36;
-		} else if (priv->bb_pre_ed_rssi <= 43) {
-			ed_inx = 18;
-			cr_201 = 0x26;
-		} else if (priv->bb_pre_ed_rssi <= 45) {
-			ed_inx = 17;
-			cr_201 = 0x18;
-		} else if (priv->bb_pre_ed_rssi <= 47) {
-			ed_inx = 16;
-			cr_201 = 0x11;
-		} else if (priv->bb_pre_ed_rssi <= 49) {
-			ed_inx = 15;
-			cr_201 = 0xa;
-		} else if (priv->bb_pre_ed_rssi <= 51) {
-			ed_inx = 14;
-			cr_201 = 0x7;
-		} else if (priv->bb_pre_ed_rssi <= 53) {
-			ed_inx = 13;
-			cr_201 = 0x4;
-		} else if (priv->bb_pre_ed_rssi <= 55) {
-			ed_inx = 12;
-			cr_201 = 0x2;
-			cr_206 = 0xc0;
-		} else if (priv->bb_pre_ed_rssi <= 56) {
-			ed_inx = 11;
-			cr_201 = 0x2;
-			cr_206 = 0x30;
-		} else if (priv->bb_pre_ed_rssi <= 57) {
-			ed_inx = 10;
-			cr_201 = 0x1;
-			cr_206 = 0xb0;
-		} else if (priv->bb_pre_ed_rssi <= 58) {
-			ed_inx = 9;
-			cr_201 = 0x1;
-			cr_206 = 0x70;
-		} else if (priv->bb_pre_ed_rssi <= 59) {
-			ed_inx = 8;
-			cr_201 = 0x1;
-			cr_206 = 0x30;
-		} else if (priv->bb_pre_ed_rssi <= 60) {
-			ed_inx = 7;
-			cr_206 = 0xea;
-		} else if (priv->bb_pre_ed_rssi <= 61) {
-			ed_inx = 6;
-			cr_206 = 0xc0;
-		} else if (priv->bb_pre_ed_rssi <= 62) {
-			ed_inx = 5;
-			cr_206 = 0x9c;
-		} else if (priv->bb_pre_ed_rssi <= 63) {
-			ed_inx = 4;
-			cr_206 = 0x80;
-		} else if (priv->bb_pre_ed_rssi <= 64) {
-			ed_inx = 3;
-			cr_206 = 0x68;
-		} else if (priv->bb_pre_ed_rssi <= 65) {
-			ed_inx = 2;
-			cr_206 = 0x52;
-		} else if (priv->bb_pre_ed_rssi <= 66) {
-			ed_inx = 1;
-			cr_206 = 0x43;
-		} else {
-			ed_inx = 0;
-			cr_206 = 0x38;
-		}
+		threshold = vt3342_vnt_threshold;
+		length = ARRAY_SIZE(vt3342_vnt_threshold);
 		break;
 	}
 
+	if (!threshold)
+		return -EINVAL;
+
+	for (ed_inx = scanning ? 0 : length - 1; ed_inx > 0; ed_inx--) {
+		if (priv->bb_pre_ed_rssi <= threshold[ed_inx].bb_pre_ed_rssi)
+			break;
+	}
+
+	cr_201 = threshold[ed_inx].cr_201;
+	cr_206 = threshold[ed_inx].cr_206;
+
 	if (ed_inx == priv->bb_pre_ed_index && !scanning)
-		return;
+		return 0;
 
 	priv->bb_pre_ed_index = ed_inx;
 
 	dev_dbg(&priv->usb->dev, "%s bb_pre_ed_rssi %d\n",
 		__func__, priv->bb_pre_ed_rssi);
 
-	if (!cr_201 && !cr_206)
-		return;
+	ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xc9, cr_201);
+	if (ret)
+		return ret;
 
-	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xc9, cr_201);
-	vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xce, cr_206);
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0xce, cr_206);
 }
 
diff --git a/drivers/staging/vt6656/baseband.h b/drivers/staging/vt6656/baseband.h
index dc42aa6ae1d9..12456ebc23ec 100644
--- a/drivers/staging/vt6656/baseband.h
+++ b/drivers/staging/vt6656/baseband.h
@@ -66,25 +66,12 @@
 #define TOP_RATE_2M         0x00200000
 #define TOP_RATE_1M         0x00100000
 
-/* Length, Service, and Signal fields of Phy for Tx */
-struct vnt_phy_field {
-	u8 signal;
-	u8 service;
-	__le16 len;
-} __packed;
-
-unsigned int vnt_get_frame_time(u8 preamble_type, u8 pkt_type,
-				unsigned int frame_length, u16 tx_rate);
-
-void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
-		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy);
-
 int vnt_set_short_slot_time(struct vnt_private *priv);
-void vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data);
+int vnt_set_vga_gain_offset(struct vnt_private *priv, u8 data);
 int vnt_set_antenna_mode(struct vnt_private *priv, u8 antenna_mode);
 int vnt_vt3184_init(struct vnt_private *priv);
 int vnt_set_deep_sleep(struct vnt_private *priv);
 int vnt_exit_deep_sleep(struct vnt_private *priv);
-void vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning);
+int vnt_update_pre_ed_threshold(struct vnt_private *priv, int scanning);
 
 #endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index dc3ab10eb630..10f3dfda83b5 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -26,7 +26,8 @@
  *
  */
 
-#include <linux/bits.h>
+#include <linux/bitops.h>
+#include <linux/errno.h>
 #include "device.h"
 #include "card.h"
 #include "baseband.h"
@@ -45,20 +46,12 @@ static const u16 cw_rxbcntsf_off[MAX_RATE] = {
 	192, 96, 34, 17, 34, 23, 17, 11, 8, 5, 4, 3
 };
 
-/*
- * Description: Set NIC media channel
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *      connection_channel  - Channel to be set
- *  Out:
- *      none
- */
-void vnt_set_channel(struct vnt_private *priv, u32 connection_channel)
+int vnt_set_channel(struct vnt_private *priv, u32 connection_channel)
 {
+	int ret;
+
 	if (connection_channel > CB_MAX_CHANNEL || !connection_channel)
-		return;
+		return -EINVAL;
 
 	/* clear NAV */
 	vnt_mac_reg_bits_on(priv, MAC_REG_MACCR, MACCR_CLRNAV);
@@ -67,284 +60,73 @@ void vnt_set_channel(struct vnt_private *priv, u32 connection_channel)
 	vnt_mac_reg_bits_off(priv, MAC_REG_CHANNEL,
 			     (BIT(7) | BIT(5) | BIT(4)));
 
-	vnt_control_out(priv, MESSAGE_TYPE_SELECT_CHANNEL,
-			connection_channel, 0, 0, NULL);
-
-	vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
-			   (u8)(connection_channel | 0x80));
-}
-
-/*
- * Description: Get CCK mode basic rate
- *
- * Parameters:
- *  In:
- *      priv		- The adapter to be set
- *      rate_idx	- Receiving data rate
- *  Out:
- *      none
- *
- * Return Value: response Control frame rate
- *
- */
-static u16 vnt_get_cck_rate(struct vnt_private *priv, u16 rate_idx)
-{
-	u16 ui = rate_idx;
-
-	while (ui > RATE_1M) {
-		if (priv->basic_rates & (1 << ui))
-			return ui;
-		ui--;
-	}
+	ret = vnt_control_out(priv, MESSAGE_TYPE_SELECT_CHANNEL,
+			      connection_channel, 0, 0, NULL);
+	if (ret)
+		return ret;
 
-	return RATE_1M;
+	return vnt_control_out_u8(priv, MESSAGE_REQUEST_MACREG, MAC_REG_CHANNEL,
+				  (u8)(connection_channel | 0x80));
 }
 
-/*
- * Description: Get OFDM mode basic rate
- *
- * Parameters:
- *  In:
- *      priv		- The adapter to be set
- *      rate_idx	- Receiving data rate
- *  Out:
- *      none
- *
- * Return Value: response Control frame rate
- *
- */
-static u16 vnt_get_ofdm_rate(struct vnt_private *priv, u16 rate_idx)
-{
-	u16 ui = rate_idx;
-
-	dev_dbg(&priv->usb->dev, "%s basic rate: %d\n",
-		__func__,  priv->basic_rates);
-
-	if (!vnt_ofdm_min_rate(priv)) {
-		dev_dbg(&priv->usb->dev, "%s (NO OFDM) %d\n",
-			__func__, rate_idx);
-		if (rate_idx > RATE_24M)
-			rate_idx = RATE_24M;
-		return rate_idx;
-	}
-
-	while (ui > RATE_11M) {
-		if (priv->basic_rates & (1 << ui)) {
-			dev_dbg(&priv->usb->dev, "%s rate: %d\n",
-				__func__, ui);
-			return ui;
-		}
-		ui--;
-	}
-
-	dev_dbg(&priv->usb->dev, "%s basic rate: 24M\n", __func__);
+static const u8 vnt_rspinf_b_short_table[] = {
+	0x70, 0x00, 0x00, 0x00, 0x38, 0x00, 0x09, 0x00,
+	0x15, 0x00, 0x0a, 0x00, 0x0b, 0x00, 0x0b, 0x80
+};
 
-	return RATE_24M;
-}
+static const u8 vnt_rspinf_b_long_table[] = {
+	0x70, 0x00, 0x00, 0x00, 0x38, 0x00, 0x01, 0x00,
+	0x15, 0x00, 0x02, 0x00, 0x0b, 0x00, 0x03, 0x80
+};
 
-/*
- * Description: Calculate TxRate and RsvTime fields for RSPINF in OFDM mode.
- *
- * Parameters:
- * In:
- *	rate	- Tx Rate
- *	bb_type	- Tx Packet type
- * Out:
- *	tx_rate	- pointer to RSPINF TxRate field
- *	rsv_time- pointer to RSPINF RsvTime field
- *
- * Return Value: none
- *
- */
-static void vnt_calculate_ofdm_rate(u16 rate, u8 bb_type,
-				    u8 *tx_rate, u8 *rsv_time)
-{
-	switch (rate) {
-	case RATE_6M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9b;
-			*rsv_time = 24;
-		} else {
-			*tx_rate = 0x8b;
-			*rsv_time = 30;
-		}
-		break;
-	case RATE_9M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9f;
-			*rsv_time = 16;
-		} else {
-			*tx_rate = 0x8f;
-			*rsv_time = 22;
-		}
-		break;
-	case RATE_12M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9a;
-			*rsv_time = 12;
-		} else {
-			*tx_rate = 0x8a;
-			*rsv_time = 18;
-		}
-		break;
-	case RATE_18M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9e;
-			*rsv_time = 8;
-		} else {
-			*tx_rate = 0x8e;
-			*rsv_time = 14;
-		}
-		break;
-	case RATE_36M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9d;
-			*rsv_time = 4;
-		} else {
-			*tx_rate = 0x8d;
-			*rsv_time = 10;
-		}
-		break;
-	case RATE_48M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x98;
-			*rsv_time = 4;
-		} else {
-			*tx_rate = 0x88;
-			*rsv_time = 10;
-		}
-		break;
-	case RATE_54M:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x9c;
-			*rsv_time = 4;
-		} else {
-			*tx_rate = 0x8c;
-			*rsv_time = 10;
-		}
-		break;
-	case RATE_24M:
-	default:
-		if (bb_type == BB_TYPE_11A) {
-			*tx_rate = 0x99;
-			*rsv_time = 8;
-		} else {
-			*tx_rate = 0x89;
-			*rsv_time = 14;
-		}
-		break;
-	}
-}
+static const u8 vnt_rspinf_a_table[] = {
+	0x9b, 0x18, 0x9f, 0x10, 0x9a, 0x0a, 0x9e, 0x08, 0x99,
+	0x08, 0x9d, 0x04, 0x98, 0x04, 0x9c, 0x04, 0x9c, 0x04
+};
 
-/*
- * Description: Set RSPINF
- *
- * Parameters:
- *  In:
- *      pDevice             - The adapter to be set
- *  Out:
- *      none
- *
- * Return Value: None.
- *
- */
+static const u8 vnt_rspinf_gb_table[] = {
+	0x8b, 0x1e, 0x8f, 0x16, 0x8a, 0x12, 0x8e, 0x0e, 0x89,
+	0x0e, 0x8d, 0x0a, 0x88, 0x0a, 0x8c, 0x0a, 0x8c, 0x0a
+};
 
-void vnt_set_rspinf(struct vnt_private *priv, u8 bb_type)
+int vnt_set_rspinf(struct vnt_private *priv, u8 bb_type)
 {
-	struct vnt_phy_field phy[4];
-	u8 tx_rate[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; /* For OFDM */
-	u8 rsv_time[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
-	u8 data[34];
-	int i;
-
-	/*RSPINF_b_1*/
-	vnt_get_phy_field(priv, 14, vnt_get_cck_rate(priv, RATE_1M),
-			  PK_TYPE_11B, &phy[0]);
-
-	/*RSPINF_b_2*/
-	vnt_get_phy_field(priv, 14, vnt_get_cck_rate(priv, RATE_2M),
-			  PK_TYPE_11B, &phy[1]);
-
-	/*RSPINF_b_5*/
-	vnt_get_phy_field(priv, 14, vnt_get_cck_rate(priv, RATE_5M),
-			  PK_TYPE_11B, &phy[2]);
-
-	/*RSPINF_b_11*/
-	vnt_get_phy_field(priv, 14, vnt_get_cck_rate(priv, RATE_11M),
-			  PK_TYPE_11B, &phy[3]);
-
-	/*RSPINF_a_6*/
-	vnt_calculate_ofdm_rate(RATE_6M, bb_type, &tx_rate[0], &rsv_time[0]);
-
-	/*RSPINF_a_9*/
-	vnt_calculate_ofdm_rate(RATE_9M, bb_type, &tx_rate[1], &rsv_time[1]);
+	const u8 *data;
+	u16 len;
+	int ret;
 
-	/*RSPINF_a_12*/
-	vnt_calculate_ofdm_rate(RATE_12M, bb_type, &tx_rate[2], &rsv_time[2]);
-
-	/*RSPINF_a_18*/
-	vnt_calculate_ofdm_rate(RATE_18M, bb_type, &tx_rate[3], &rsv_time[3]);
-
-	/*RSPINF_a_24*/
-	vnt_calculate_ofdm_rate(RATE_24M, bb_type, &tx_rate[4], &rsv_time[4]);
-
-	/*RSPINF_a_36*/
-	vnt_calculate_ofdm_rate(vnt_get_ofdm_rate(priv, RATE_36M),
-				bb_type, &tx_rate[5], &rsv_time[5]);
-
-	/*RSPINF_a_48*/
-	vnt_calculate_ofdm_rate(vnt_get_ofdm_rate(priv, RATE_48M),
-				bb_type, &tx_rate[6], &rsv_time[6]);
-
-	/*RSPINF_a_54*/
-	vnt_calculate_ofdm_rate(vnt_get_ofdm_rate(priv, RATE_54M),
-				bb_type, &tx_rate[7], &rsv_time[7]);
-
-	/*RSPINF_a_72*/
-	vnt_calculate_ofdm_rate(vnt_get_ofdm_rate(priv, RATE_54M),
-				bb_type, &tx_rate[8], &rsv_time[8]);
-
-	put_unaligned(phy[0].len, (u16 *)&data[0]);
-	data[2] = phy[0].signal;
-	data[3] = phy[0].service;
-
-	put_unaligned(phy[1].len, (u16 *)&data[4]);
-	data[6] = phy[1].signal;
-	data[7] = phy[1].service;
-
-	put_unaligned(phy[2].len, (u16 *)&data[8]);
-	data[10] = phy[2].signal;
-	data[11] = phy[2].service;
+	if (priv->preamble_type) {
+		data = vnt_rspinf_b_short_table;
+		len = ARRAY_SIZE(vnt_rspinf_b_short_table);
+	} else {
+		data = vnt_rspinf_b_long_table;
+		len = ARRAY_SIZE(vnt_rspinf_b_long_table);
+	}
 
-	put_unaligned(phy[3].len, (u16 *)&data[12]);
-	data[14] = phy[3].signal;
-	data[15] = phy[3].service;
+	 /* RSPINF_b_1 to RSPINF_b_11 */
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_RSPINF_B_1,
+			      MESSAGE_REQUEST_MACREG, len, data);
+	if (ret)
+		return ret;
 
-	for (i = 0; i < 9; i++) {
-		data[16 + i * 2] = tx_rate[i];
-		data[16 + i * 2 + 1] = rsv_time[i];
+	if (bb_type == BB_TYPE_11A) {
+		data = vnt_rspinf_a_table;
+		len = ARRAY_SIZE(vnt_rspinf_a_table);
+	} else {
+		data = vnt_rspinf_gb_table;
+		len = ARRAY_SIZE(vnt_rspinf_gb_table);
 	}
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_RSPINF_B_1,
-			MESSAGE_REQUEST_MACREG, 34, &data[0]);
+	/* RSPINF_a_6 to RSPINF_a_72 */
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_RSPINF_A_6,
+			       MESSAGE_REQUEST_MACREG, len, data);
 }
 
-/*
- * Description: Update IFS
- *
- * Parameters:
- *  In:
- *	priv - The adapter to be set
- * Out:
- *	none
- *
- * Return Value: None.
- *
- */
-void vnt_update_ifs(struct vnt_private *priv)
+int vnt_update_ifs(struct vnt_private *priv)
 {
 	u8 max_min = 0;
 	u8 data[4];
+	int ret;
 
 	if (priv->packet_type == PK_TYPE_11A) {
 		priv->slot = C_SLOT_SHORT;
@@ -367,89 +149,36 @@ void vnt_update_ifs(struct vnt_private *priv)
 
 	priv->eifs = C_EIFS;
 
-	switch (priv->rf_type) {
-	case RF_VT3226D0:
-		if (priv->bb_type != BB_TYPE_11B) {
-			priv->sifs -= 1;
-			priv->difs -= 1;
-			break;
-		}
-		/* fall through */
-	case RF_AIROHA7230:
-	case RF_AL2230:
-	case RF_AL2230S:
-		if (priv->bb_type != BB_TYPE_11B)
-			break;
-		/* fall through */
-	case RF_RFMD2959:
-	case RF_VT3226:
-	case RF_VT3342A0:
-		priv->sifs -= 3;
-		priv->difs -= 3;
-		break;
-	case RF_MAXIM2829:
-		if (priv->bb_type == BB_TYPE_11A) {
-			priv->sifs -= 5;
-			priv->difs -= 5;
-		} else {
-			priv->sifs -= 2;
-			priv->difs -= 2;
-		}
-
-		break;
-	}
-
 	data[0] = (u8)priv->sifs;
 	data[1] = (u8)priv->difs;
 	data[2] = (u8)priv->eifs;
 	data[3] = (u8)priv->slot;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_SIFS,
-			MESSAGE_REQUEST_MACREG, 4, &data[0]);
+	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_SIFS,
+			      MESSAGE_REQUEST_MACREG, 4, &data[0]);
+	if (ret)
+		return ret;
 
 	max_min |= 0xa0;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_CWMAXMIN0,
-			MESSAGE_REQUEST_MACREG, 1, &max_min);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_CWMAXMIN0,
+			       MESSAGE_REQUEST_MACREG, 1, &max_min);
 }
 
 void vnt_update_top_rates(struct vnt_private *priv)
 {
-	u8 top_ofdm = RATE_24M, top_cck = RATE_1M;
-	u8 i;
-
-	/*Determines the highest basic rate.*/
-	for (i = RATE_54M; i >= RATE_6M; i--) {
-		if (priv->basic_rates & (u16)(1 << i)) {
-			top_ofdm = i;
-			break;
-		}
-	}
-
-	priv->top_ofdm_basic_rate = top_ofdm;
+	int pos;
 
-	for (i = RATE_11M;; i--) {
-		if (priv->basic_rates & (u16)(1 << i)) {
-			top_cck = i;
-			break;
-		}
-		if (i == RATE_1M)
-			break;
-	}
+	pos = fls(priv->basic_rates & GENMASK(RATE_54M, RATE_6M));
+	priv->top_ofdm_basic_rate = pos ? (pos - 1) : RATE_24M;
 
-	priv->top_cck_basic_rate = top_cck;
+	pos = fls(priv->basic_rates & GENMASK(RATE_11M, RATE_1M));
+	priv->top_cck_basic_rate = pos ? (pos - 1) : RATE_1M;
 }
 
-int vnt_ofdm_min_rate(struct vnt_private *priv)
+bool vnt_ofdm_min_rate(struct vnt_private *priv)
 {
-	int ii;
-
-	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
-		if ((priv->basic_rates) & ((u16)BIT(ii)))
-			return true;
-	}
-
-	return false;
+	return priv->basic_rates & GENMASK(RATE_54M, RATE_6M) ? true : false;
 }
 
 u8 vnt_get_pkt_type(struct vnt_private *priv)
@@ -481,23 +210,8 @@ u64 vnt_get_tsf_offset(u8 rx_rate, u64 tsf1, u64 tsf2)
 	return tsf1 - tsf2 - (u64)cw_rxbcntsf_off[rx_rate % MAX_RATE];
 }
 
-/*
- * Description: Sync. TSF counter to BSS
- *              Get TSF offset and write to HW
- *
- * Parameters:
- *  In:
- *      priv		- The adapter to be sync.
- *      time_stamp	- Rx BCN's TSF
- *      local_tsf	- Local TSF
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
-		    u64 time_stamp, u64 local_tsf)
+int vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
+		   u64 time_stamp, u64 local_tsf)
 {
 	u64 tsf_offset = 0;
 	u8 data[8];
@@ -513,8 +227,8 @@ void vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
 	data[6] = (u8)(tsf_offset >> 48);
 	data[7] = (u8)(tsf_offset >> 56);
 
-	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
-			MESSAGE_REQUEST_TSF, 0, 8, data);
+	return vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			       MESSAGE_REQUEST_TSF, 0, 8, data);
 }
 
 /*
@@ -589,21 +303,7 @@ u64 vnt_get_next_tbtt(u64 tsf, u16 beacon_interval)
 	return tsf;
 }
 
-/*
- * Description: Set NIC TSF counter for first Beacon time
- *              Get NEXTTBTT from adjusted TSF and Beacon Interval
- *
- * Parameters:
- *  In:
- *      dwIoBase        - IO Base
- *	beacon_interval - Beacon Interval
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval)
+int vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval)
 {
 	u64 next_tbtt = 0;
 	u8 data[8];
@@ -621,29 +321,15 @@ void vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval)
 	data[6] = (u8)(next_tbtt >> 48);
 	data[7] = (u8)(next_tbtt >> 56);
 
-	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
-			MESSAGE_REQUEST_TBTT, 0, 8, data);
+	return vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			       MESSAGE_REQUEST_TBTT, 0, 8, data);
 }
 
-/*
- * Description: Sync NIC TSF counter for Beacon time
- *              Get NEXTTBTT and write to HW
- *
- * Parameters:
- *  In:
- *	priv		- The adapter to be set
- *      tsf		- Current TSF counter
- *      beacon_interval - Beacon Interval
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
-			  u16 beacon_interval)
+int vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
+			 u16 beacon_interval)
 {
 	u8 data[8];
+	int ret;
 
 	tsf = vnt_get_next_tbtt(tsf, beacon_interval);
 
@@ -656,10 +342,13 @@ void vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
 	data[6] = (u8)(tsf >> 48);
 	data[7] = (u8)(tsf >> 56);
 
-	vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
-			MESSAGE_REQUEST_TBTT, 0, 8, data);
+	ret = vnt_control_out(priv, MESSAGE_TYPE_SET_TSFTBTT,
+			      MESSAGE_REQUEST_TBTT, 0, 8, data);
+	if (ret)
+		return ret;
 
 	dev_dbg(&priv->usb->dev, "%s TBTT: %8llx\n", __func__, tsf);
+	return 0;
 }
 
 /*
@@ -723,9 +412,13 @@ int vnt_radio_power_on(struct vnt_private *priv)
 {
 	int ret = 0;
 
-	vnt_exit_deep_sleep(priv);
+	ret = vnt_exit_deep_sleep(priv);
+	if (ret)
+		return ret;
 
-	vnt_mac_reg_bits_on(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
+	ret = vnt_mac_reg_bits_on(priv, MAC_REG_HOSTCR, HOSTCR_RXON);
+	if (ret)
+		return ret;
 
 	switch (priv->rf_type) {
 	case RF_AL2230:
@@ -734,56 +427,69 @@ int vnt_radio_power_on(struct vnt_private *priv)
 	case RF_VT3226:
 	case RF_VT3226D0:
 	case RF_VT3342A0:
-		vnt_mac_reg_bits_on(priv, MAC_REG_SOFTPWRCTL,
-				    (SOFTPWRCTL_SWPE2 | SOFTPWRCTL_SWPE3));
-		break;
+		ret = vnt_mac_reg_bits_on(priv, MAC_REG_SOFTPWRCTL,
+					  (SOFTPWRCTL_SWPE2 |
+					   SOFTPWRCTL_SWPE3));
+		if (ret)
+			return ret;
 	}
 
-	vnt_mac_reg_bits_off(priv, MAC_REG_GPIOCTL1, GPIO3_INTMD);
-
-	return ret;
+	return vnt_mac_reg_bits_off(priv, MAC_REG_GPIOCTL1, GPIO3_INTMD);
 }
 
-void vnt_set_bss_mode(struct vnt_private *priv)
+int vnt_set_bss_mode(struct vnt_private *priv)
 {
-	if (priv->rf_type == RF_AIROHA7230 && priv->bb_type == BB_TYPE_11A)
-		vnt_mac_set_bb_type(priv, BB_TYPE_11G);
-	else
-		vnt_mac_set_bb_type(priv, priv->bb_type);
+	int ret;
+	unsigned char type = priv->bb_type;
+	unsigned char data = 0;
+	unsigned char bb_vga_0 = 0x1c;
+	unsigned char bb_vga_2_3 = 0x00;
 
-	priv->packet_type = vnt_get_pkt_type(priv);
+	if (priv->rf_type == RF_AIROHA7230 && priv->bb_type == BB_TYPE_11A)
+		type = BB_TYPE_11G;
 
-	if (priv->bb_type == BB_TYPE_11A)
-		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x03);
-	else if (priv->bb_type == BB_TYPE_11B)
-		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x02);
-	else if (priv->bb_type == BB_TYPE_11G)
-		vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG, 0x88, 0x08);
+	ret = vnt_mac_set_bb_type(priv, type);
+	if (ret)
+		return ret;
 
-	vnt_update_ifs(priv);
-	vnt_set_rspinf(priv, (u8)priv->bb_type);
+	priv->packet_type = vnt_get_pkt_type(priv);
 
 	if (priv->bb_type == BB_TYPE_11A) {
-		if (priv->rf_type == RF_AIROHA7230) {
-			priv->bb_vga[0] = 0x20;
+		data = 0x03;
+		bb_vga_0 = 0x20;
+		bb_vga_2_3 = 0x10;
+	} else if (priv->bb_type == BB_TYPE_11B) {
+		data = 0x02;
+	} else if (priv->bb_type == BB_TYPE_11G) {
+		data = 0x08;
+	}
 
-			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
-					   0xe7, priv->bb_vga[0]);
-		}
+	if (data) {
+		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+					 0x88, data);
+		if (ret)
+			return ret;
+	}
 
-		priv->bb_vga[2] = 0x10;
-		priv->bb_vga[3] = 0x10;
-	} else {
-		if (priv->rf_type == RF_AIROHA7230) {
-			priv->bb_vga[0] = 0x1c;
+	ret = vnt_update_ifs(priv);
+	if (ret)
+		return ret;
 
-			vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
-					   0xe7, priv->bb_vga[0]);
-		}
+	ret = vnt_set_rspinf(priv, priv->bb_type);
+	if (ret)
+		return ret;
 
-		priv->bb_vga[2] = 0x0;
-		priv->bb_vga[3] = 0x0;
+	if (priv->rf_type == RF_AIROHA7230) {
+		priv->bb_vga[0] = bb_vga_0;
+
+		ret = vnt_control_out_u8(priv, MESSAGE_REQUEST_BBREG,
+					 0xe7, priv->bb_vga[0]);
+		if (ret)
+			return ret;
 	}
 
-	vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
+	priv->bb_vga[2] = bb_vga_2_3;
+	priv->bb_vga[3] = bb_vga_2_3;
+
+	return vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
 }
diff --git a/drivers/staging/vt6656/card.h b/drivers/staging/vt6656/card.h
index 75cd340c0cce..a524fdc60ae3 100644
--- a/drivers/staging/vt6656/card.h
+++ b/drivers/staging/vt6656/card.h
@@ -25,23 +25,23 @@
 
 struct vnt_private;
 
-void vnt_set_channel(struct vnt_private *priv, u32 connection_channel);
-void vnt_set_rspinf(struct vnt_private *priv, u8 bb_type);
-void vnt_update_ifs(struct vnt_private *priv);
+int vnt_set_channel(struct vnt_private *priv, u32 connection_channel);
+int vnt_set_rspinf(struct vnt_private *priv, u8 bb_type);
+int vnt_update_ifs(struct vnt_private *priv);
 void vnt_update_top_rates(struct vnt_private *priv);
-int vnt_ofdm_min_rate(struct vnt_private *priv);
-void vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
-		    u64 time_stamp, u64 local_tsf);
+bool vnt_ofdm_min_rate(struct vnt_private *priv);
+int vnt_adjust_tsf(struct vnt_private *priv, u8 rx_rate,
+		   u64 time_stamp, u64 local_tsf);
 bool vnt_get_current_tsf(struct vnt_private *priv, u64 *current_tsf);
 bool vnt_clear_current_tsf(struct vnt_private *priv);
-void vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval);
-void vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
-			  u16 beacon_interval);
+int vnt_reset_next_tbtt(struct vnt_private *priv, u16 beacon_interval);
+int vnt_update_next_tbtt(struct vnt_private *priv, u64 tsf,
+			 u16 beacon_interval);
 u64 vnt_get_next_tbtt(u64 tsf, u16 beacon_interval);
 u64 vnt_get_tsf_offset(u8 rx_rate, u64 tsf1, u64 tsf2);
 int vnt_radio_power_off(struct vnt_private *priv);
 int vnt_radio_power_on(struct vnt_private *priv);
 u8 vnt_get_pkt_type(struct vnt_private *priv);
-void vnt_set_bss_mode(struct vnt_private *priv);
+int vnt_set_bss_mode(struct vnt_private *priv);
 
 #endif /* __CARD_H__ */
diff --git a/drivers/staging/vt6656/device.h b/drivers/staging/vt6656/device.h
index e6ee9411f080..947530fefe94 100644
--- a/drivers/staging/vt6656/device.h
+++ b/drivers/staging/vt6656/device.h
@@ -73,6 +73,10 @@
 
 #define DEVICE_VERSION			"mac80211"
 
+#define FIRMWARE_VERSION		0x133		/* version 1.51 */
+#define FIRMWARE_NAME			"vntwusb.fw"
+#define FIRMWARE_CHUNK_SIZE		0x400
+
 #define CONFIG_PATH			"/etc/vntconfiguration.dat"
 
 #define MAX_UINTS			8
@@ -202,8 +206,7 @@ struct vnt_rsp_card_init {
  * Enum of context types for SendPacket
  */
 enum {
-	CONTEXT_DATA_PACKET = 1,
-	CONTEXT_MGMT_PACKET,
+	CONTEXT_DATA_PACKET = 0,
 	CONTEXT_BEACON_PACKET
 };
 
@@ -234,18 +237,14 @@ struct vnt_rcb {
 struct vnt_usb_send_context {
 	void *priv;
 	struct sk_buff *skb;
-	struct urb *urb;
-	struct ieee80211_hdr *hdr;
-	unsigned int buf_len;
+	void *tx_buffer;
 	u32 frame_len;
 	u16 tx_hdr_size;
 	u16 tx_rate;
 	u8 type;
 	u8 pkt_no;
 	u8 pkt_type;
-	u8 need_ack;
 	bool in_use;
-	unsigned char data[MAX_TOTAL_SIZE_WITH_ALL_HEADERS];
 };
 
 /*
@@ -288,6 +287,7 @@ struct vnt_private {
 
 	/* Variables to track resources for the BULK Out Pipe */
 	struct vnt_usb_send_context *tx_context[CB_MAX_TX_DESC];
+	struct usb_anchor tx_submitted;
 	u32 num_tx_context;
 
 	/* Variables to track resources for the Interrupt In Pipe */
@@ -344,13 +344,9 @@ struct vnt_private {
 	u8 ofdm_pwr_tbl[14];
 	u8 ofdm_a_pwr_tbl[42];
 
-	u16 current_rate;
 	u16 tx_rate_fb0;
 	u16 tx_rate_fb1;
 
-	u8 short_retry_limit;
-	u8 long_retry_limit;
-
 	enum nl80211_iftype op_mode;
 
 	int short_slot_time;
@@ -383,8 +379,6 @@ struct vnt_private {
 	u8 bb_pre_ed_rssi;
 	u8 bb_pre_ed_index;
 
-	u16 wake_up_count;
-
 	/* command timer */
 	struct delayed_work run_command_work;
 
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
deleted file mode 100644
index 70358d427211..000000000000
--- a/drivers/staging/vt6656/firmware.c
+++ /dev/null
@@ -1,106 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0+
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * File: baseband.c
- *
- * Purpose: Implement functions to access baseband
- *
- * Author: Yiching Chen
- *
- * Date: May 20, 2004
- *
- * Functions:
- *
- * Revision History:
- *
- */
-
-#include <linux/compiler.h>
-#include "firmware.h"
-#include "usbpipe.h"
-
-#define FIRMWARE_VERSION	0x133		/* version 1.51 */
-#define FIRMWARE_NAME		"vntwusb.fw"
-
-#define FIRMWARE_CHUNK_SIZE	0x400
-
-int vnt_download_firmware(struct vnt_private *priv)
-{
-	struct device *dev = &priv->usb->dev;
-	const struct firmware *fw;
-	u16 length;
-	int ii;
-	int ret = 0;
-
-	dev_dbg(dev, "---->Download firmware\n");
-
-	ret = request_firmware(&fw, FIRMWARE_NAME, dev);
-	if (ret) {
-		dev_err(dev, "firmware file %s request failed (%d)\n",
-			FIRMWARE_NAME, ret);
-		goto end;
-	}
-
-	for (ii = 0; ii < fw->size; ii += FIRMWARE_CHUNK_SIZE) {
-		length = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
-
-		ret = vnt_control_out(priv, 0, 0x1200 + ii, 0x0000, length,
-				      fw->data + ii);
-		if (ret)
-			goto free_fw;
-
-		dev_dbg(dev, "Download firmware...%d %zu\n", ii, fw->size);
-	}
-
-free_fw:
-	release_firmware(fw);
-end:
-	return ret;
-}
-MODULE_FIRMWARE(FIRMWARE_NAME);
-
-int vnt_firmware_branch_to_sram(struct vnt_private *priv)
-{
-	dev_dbg(&priv->usb->dev, "---->Branch to Sram\n");
-
-	return vnt_control_out(priv, 1, 0x1200, 0x0000, 0, NULL);
-}
-
-int vnt_check_firmware_version(struct vnt_private *priv)
-{
-	int ret = 0;
-
-	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, 0,
-			     MESSAGE_REQUEST_VERSION, 2,
-			     (u8 *)&priv->firmware_version);
-	if (ret) {
-		dev_dbg(&priv->usb->dev,
-			"Could not get firmware version: %d.\n", ret);
-		goto end;
-	}
-
-	dev_dbg(&priv->usb->dev, "Firmware Version [%04x]\n",
-		priv->firmware_version);
-
-	if (priv->firmware_version == 0xFFFF) {
-		dev_dbg(&priv->usb->dev, "In Loader.\n");
-		ret = -EINVAL;
-		goto end;
-	}
-
-	if (priv->firmware_version < FIRMWARE_VERSION) {
-		/* branch to loader for download new firmware */
-		ret = vnt_firmware_branch_to_sram(priv);
-		if (ret) {
-			dev_dbg(&priv->usb->dev,
-				"Could not branch to SRAM: %d.\n", ret);
-		} else {
-			ret = -EINVAL;
-		}
-	}
-
-end:
-	return ret;
-}
diff --git a/drivers/staging/vt6656/firmware.h b/drivers/staging/vt6656/firmware.h
deleted file mode 100644
index 161126faf396..000000000000
--- a/drivers/staging/vt6656/firmware.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0+ */
-/*
- * Copyright (c) 1996, 2003 VIA Networking Technologies, Inc.
- * All rights reserved.
- *
- * File: firmware.h
- *
- * Purpose: Version and Release Information
- *
- * Author: Yiching Chen
- *
- * Date: May 20, 2004
- *
- */
-
-#ifndef __FIRMWARE_H__
-#define __FIRMWARE_H__
-
-#include "device.h"
-
-int vnt_download_firmware(struct vnt_private *priv);
-int vnt_firmware_branch_to_sram(struct vnt_private *priv);
-int vnt_check_firmware_version(struct vnt_private *priv);
-
-#endif /* __FIRMWARE_H__ */
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index ac3b188984d0..c66cb53cfc09 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -35,7 +35,7 @@ int vnt_key_init_table(struct vnt_private *priv)
 
 static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
 			   struct ieee80211_key_conf *key, u32 key_type,
-			   u32 mode, bool onfly_latch)
+			   u32 mode)
 {
 	struct vnt_private *priv = hw->priv;
 	u8 broadcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
@@ -68,17 +68,11 @@ static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
 		entry = MAX_KEY_TABLE - 1;
 		key->hw_key_idx = entry;
 		/* fall through */
-	case VNT_KEY_ALLGROUP:
-		key_mode |= VNT_KEY_ALLGROUP;
-		if (onfly_latch)
-			key_mode |= VNT_KEY_ONFLY_ALL;
-		/* fall through */
 	case VNT_KEY_GROUP_ADDRESS:
-		key_mode |= mode;
-		/* fall through */
+		key_mode = mode | (mode << 4);
+		break;
 	case VNT_KEY_GROUP:
-		key_mode |= (mode << 4);
-		key_mode |= VNT_KEY_GROUP;
+		key_mode = mode << 4;
 		break;
 	case  VNT_KEY_PAIRWISE:
 		key_mode |= mode;
@@ -88,8 +82,7 @@ static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
 		return -EINVAL;
 	}
 
-	if (onfly_latch)
-		key_mode |= VNT_KEY_ONFLY;
+	key_mode |= key_type;
 
 	if (mode == KEY_CTL_WEP) {
 		if (key->keylen == WLAN_KEY_LEN_WEP40)
@@ -98,9 +91,8 @@ static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
 			key->key[15] |= 0x80;
 	}
 
-	vnt_mac_set_keyentry(priv, key_mode, entry, key_inx, bssid, key->key);
-
-	return 0;
+	return vnt_mac_set_keyentry(priv, key_mode, entry,
+				    key_inx, bssid, key->key);
 }
 
 int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
@@ -109,28 +101,21 @@ int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 	struct vnt_private *priv = hw->priv;
 	u8 *mac_addr = NULL;
 	u8 key_dec_mode = 0;
-	int ret = 0, u;
 
 	if (sta)
 		mac_addr = &sta->addr[0];
 
 	switch (key->cipher) {
-	case 0:
-		for (u = 0 ; u < MAX_KEY_TABLE; u++)
-			vnt_mac_disable_keyentry(priv, u);
-		return ret;
-
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
-		for (u = 0; u < MAX_KEY_TABLE; u++)
-			vnt_mac_disable_keyentry(priv, u);
-
 		vnt_set_keymode(hw, mac_addr, key, VNT_KEY_DEFAULTKEY,
-				KEY_CTL_WEP, true);
+				KEY_CTL_WEP);
 
 		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 
-		return ret;
+		return vnt_set_keymode(hw, mac_addr, key, VNT_KEY_DEFAULTKEY,
+				       KEY_CTL_WEP);
+
 	case WLAN_CIPHER_SUITE_TKIP:
 		key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
 		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
@@ -151,11 +136,9 @@ int vnt_set_keys(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 	}
 
 	if (key->flags & IEEE80211_KEY_FLAG_PAIRWISE)
-		vnt_set_keymode(hw, mac_addr, key, VNT_KEY_PAIRWISE,
-				key_dec_mode, true);
-	else
-		vnt_set_keymode(hw, mac_addr, key, VNT_KEY_GROUP_ADDRESS,
-				key_dec_mode, true);
+		return vnt_set_keymode(hw, mac_addr, key, VNT_KEY_PAIRWISE,
+				       key_dec_mode);
 
-	return 0;
+	return vnt_set_keymode(hw, mac_addr, key,
+				VNT_KEY_GROUP_ADDRESS, key_dec_mode);
 }
diff --git a/drivers/staging/vt6656/key.h b/drivers/staging/vt6656/key.h
index 918c07cf86cd..1f3449e66143 100644
--- a/drivers/staging/vt6656/key.h
+++ b/drivers/staging/vt6656/key.h
@@ -25,13 +25,14 @@
 #define KEY_CTL_TKIP        0x02
 #define KEY_CTL_CCMP        0x03
 
-#define VNT_KEY_DEFAULTKEY	0x1
-#define VNT_KEY_GROUP_ADDRESS	0x2
-#define VNT_KEY_ALLGROUP	0x4
-#define VNT_KEY_GROUP		0x40
-#define VNT_KEY_PAIRWISE	0x00
-#define VNT_KEY_ONFLY		0x8000
 #define VNT_KEY_ONFLY_ALL	0x4000
+#define VNT_KEY_ONFLY		0x8000
+#define VNT_KEY_ALLGROUP	0x04
+#define VNT_KEY_GROUP		0x40
+#define VNT_KEY_PAIRWISE	VNT_KEY_ONFLY
+#define VNT_KEY_GROUP_ADDRESS	(VNT_KEY_ALLGROUP | VNT_KEY_GROUP)
+#define VNT_KEY_DEFAULTKEY	(VNT_KEY_GROUP_ADDRESS | VNT_KEY_ONFLY |\
+				VNT_KEY_ONFLY_ALL)
 
 int vnt_key_init_table(struct vnt_private *priv);
 
diff --git a/drivers/staging/vt6656/mac.c b/drivers/staging/vt6656/mac.c
index 5cacf6e60e90..da7067c34643 100644
--- a/drivers/staging/vt6656/mac.c
+++ b/drivers/staging/vt6656/mac.c
@@ -22,90 +22,40 @@
 #include "mac.h"
 #include "usbpipe.h"
 
-/*
- * Description:
- *      Write MAC Multicast Address Mask
- *
- * Parameters:
- *  In:
- *	mc_filter (mac filter)
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter)
+int vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter)
 {
 	__le64 le_mc = cpu_to_le64(mc_filter);
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
-			MESSAGE_REQUEST_MACREG, sizeof(le_mc), (u8 *)&le_mc);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_MAR0,
+			       MESSAGE_REQUEST_MACREG, sizeof(le_mc),
+			       (u8 *)&le_mc);
 }
 
-/*
- * Description:
- *      Shut Down MAC
- *
- * Parameters:
- *  In:
- *  Out:
- *      none
- *
- *
- */
-void vnt_mac_shutdown(struct vnt_private *priv)
+int vnt_mac_shutdown(struct vnt_private *priv)
 {
-	vnt_control_out(priv, MESSAGE_TYPE_MACSHUTDOWN, 0, 0, 0, NULL);
+	return vnt_control_out(priv, MESSAGE_TYPE_MACSHUTDOWN, 0, 0, 0, NULL);
 }
 
-void vnt_mac_set_bb_type(struct vnt_private *priv, u8 type)
+int vnt_mac_set_bb_type(struct vnt_private *priv, u8 type)
 {
 	u8 data[2];
 
 	data[0] = type;
 	data[1] = EnCFG_BBType_MASK;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
-			MESSAGE_REQUEST_MACREG,	ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG,	ARRAY_SIZE(data),
+			       data);
 }
 
-/*
- * Description:
- *      Disable the Key Entry by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx)
+int vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx)
 {
-	vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY, 0, 0,
-			sizeof(entry_idx), &entry_idx);
+	return vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY, 0, 0,
+			       sizeof(entry_idx), &entry_idx);
 }
 
-/*
- * Description:
- *      Set the Key by MISCFIFO
- *
- * Parameters:
- *  In:
- *      dwIoBase        - Base Address for MAC
- *
- *  Out:
- *      none
- *
- * Return Value: none
- *
- */
-void vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
-			  u32 key_idx, u8 *addr, u8 *key)
+int vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+			 u32 key_idx, u8 *addr, u8 *key)
 {
 	struct vnt_mac_set_key set_key;
 	u16 offset;
@@ -124,9 +74,9 @@ void vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
 	dev_dbg(&priv->usb->dev, "offset %d key ctl %d set key %24ph\n",
 		offset, key_ctl, (u8 *)&set_key);
 
-	vnt_control_out(priv, MESSAGE_TYPE_SETKEY, offset,
-			(u16)key_idx, sizeof(struct vnt_mac_set_key),
-			(u8 *)&set_key);
+	return vnt_control_out(priv, MESSAGE_TYPE_SETKEY, offset,
+			       (u16)key_idx, sizeof(struct vnt_mac_set_key),
+			       (u8 *)&set_key);
 }
 
 int vnt_mac_reg_bits_off(struct vnt_private *priv, u8 reg_ofs, u8 bits)
@@ -151,76 +101,76 @@ int vnt_mac_reg_bits_on(struct vnt_private *priv, u8 reg_ofs, u8 bits)
 			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word)
+int vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word)
 {
 	u8 data[2];
 
 	data[0] = (u8)(word & 0xff);
 	data[1] = (u8)(word >> 8);
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, reg_ofs,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, reg_ofs,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr)
+int vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr)
 {
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BSSID0,
-			MESSAGE_REQUEST_MACREG, ETH_ALEN, addr);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BSSID0,
+			       MESSAGE_REQUEST_MACREG, ETH_ALEN, addr);
 }
 
-void vnt_mac_enable_protect_mode(struct vnt_private *priv)
+int vnt_mac_enable_protect_mode(struct vnt_private *priv)
 {
 	u8 data[2];
 
 	data[0] = EnCFG_ProtectMd;
 	data[1] = EnCFG_ProtectMd;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_disable_protect_mode(struct vnt_private *priv)
+int vnt_mac_disable_protect_mode(struct vnt_private *priv)
 {
 	u8 data[2];
 
 	data[0] = 0;
 	data[1] = EnCFG_ProtectMd;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG0,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv)
+int vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv)
 {
 	u8 data[2];
 
 	data[0] = EnCFG_BarkerPream;
 	data[1] = EnCFG_BarkerPream;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv)
+int vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv)
 {
 	u8 data[2];
 
 	data[0] = 0;
 	data[1] = EnCFG_BarkerPream;
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_MASK, MAC_REG_ENCFG2,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
-void vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval)
+int vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval)
 {
 	u8 data[2];
 
 	data[0] = (u8)(interval & 0xff);
 	data[1] = (u8)(interval >> 8);
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BI,
-			MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE, MAC_REG_BI,
+			       MESSAGE_REQUEST_MACREG, ARRAY_SIZE(data), data);
 }
 
 int vnt_mac_set_led(struct vnt_private *priv, u8 state, u8 led)
diff --git a/drivers/staging/vt6656/mac.h b/drivers/staging/vt6656/mac.h
index c532b27de37f..dae70b5c7634 100644
--- a/drivers/staging/vt6656/mac.h
+++ b/drivers/staging/vt6656/mac.h
@@ -177,7 +177,7 @@
 #define EnCFG_BBType_a		0x00
 #define EnCFG_BBType_b		BIT(0)
 #define EnCFG_BBType_g		BIT(1)
-#define EnCFG_BBType_MASK	(BIT(0) | BIT(1))
+#define EnCFG_BBType_MASK	(EnCFG_BBType_b | EnCFG_BBType_g)
 #define EnCFG_ProtectMd		BIT(5)
 
 /* Bits in the EnhanceCFG_1 register */
@@ -355,21 +355,21 @@ struct vnt_mac_set_key {
 	u8 key[WLAN_KEY_LEN_CCMP];
 } __packed;
 
-void vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter);
-void vnt_mac_shutdown(struct vnt_private *priv);
-void vnt_mac_set_bb_type(struct vnt_private *priv, u8 type);
-void vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx);
-void vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
-			  u32 key_idx, u8 *addr, u8 *key);
+int vnt_mac_set_filter(struct vnt_private *priv, u64 mc_filter);
+int vnt_mac_shutdown(struct vnt_private *priv);
+int vnt_mac_set_bb_type(struct vnt_private *priv, u8 type);
+int vnt_mac_disable_keyentry(struct vnt_private *priv, u8 entry_idx);
+int vnt_mac_set_keyentry(struct vnt_private *priv, u16 key_ctl, u32 entry_idx,
+			 u32 key_idx, u8 *addr, u8 *key);
 int vnt_mac_reg_bits_off(struct vnt_private *priv, u8 reg_ofs, u8 bits);
 int vnt_mac_reg_bits_on(struct vnt_private *priv, u8 reg_ofs, u8 bits);
-void vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word);
-void vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr);
-void vnt_mac_enable_protect_mode(struct vnt_private *priv);
-void vnt_mac_disable_protect_mode(struct vnt_private *priv);
-void vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv);
-void vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv);
-void vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval);
+int vnt_mac_write_word(struct vnt_private *priv, u8 reg_ofs, u16 word);
+int vnt_mac_set_bssid_addr(struct vnt_private *priv, u8 *addr);
+int vnt_mac_enable_protect_mode(struct vnt_private *priv);
+int vnt_mac_disable_protect_mode(struct vnt_private *priv);
+int vnt_mac_enable_barker_preamble_mode(struct vnt_private *priv);
+int vnt_mac_disable_barker_preamble_mode(struct vnt_private *priv);
+int vnt_mac_set_beacon_interval(struct vnt_private *priv, u16 interval);
 int vnt_mac_set_led(struct vnt_private *privpriv, u8 state, u8 led);
 
 #endif /* __MAC_H__ */
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 5f78cad3b647..8bf851c53f4e 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -33,7 +33,6 @@
 #include "wcmd.h"
 #include "rxtx.h"
 #include "rf.h"
-#include "firmware.h"
 #include "usbpipe.h"
 #include "channel.h"
 
@@ -60,8 +59,6 @@ MODULE_PARM_DESC(tx_buffers, "Number of receive usb tx buffers");
 
 #define RTS_THRESH_DEF     2347
 #define FRAG_THRESH_DEF     2346
-#define SHORT_RETRY_DEF     8
-#define LONG_RETRY_DEF     4
 
 /* BasebandType[] baseband type selected
  * 0: indicate 802.11a type
@@ -94,15 +91,91 @@ static void vnt_set_options(struct vnt_private *priv)
 	else
 		priv->num_rcb = vnt_rx_buffers;
 
-	priv->short_retry_limit = SHORT_RETRY_DEF;
-	priv->long_retry_limit = LONG_RETRY_DEF;
 	priv->op_mode = NL80211_IFTYPE_UNSPECIFIED;
 	priv->bb_type = BBP_TYPE_DEF;
 	priv->packet_type = priv->bb_type;
-	priv->preamble_type = 0;
+	priv->preamble_type = PREAMBLE_LONG;
 	priv->exist_sw_net_addr = false;
 }
 
+static int vnt_download_firmware(struct vnt_private *priv)
+{
+	struct device *dev = &priv->usb->dev;
+	const struct firmware *fw;
+	u16 length;
+	int ii;
+	int ret = 0;
+
+	dev_dbg(dev, "---->Download firmware\n");
+
+	ret = request_firmware(&fw, FIRMWARE_NAME, dev);
+	if (ret) {
+		dev_err(dev, "firmware file %s request failed (%d)\n",
+			FIRMWARE_NAME, ret);
+		goto end;
+	}
+
+	for (ii = 0; ii < fw->size; ii += FIRMWARE_CHUNK_SIZE) {
+		length = min_t(int, fw->size - ii, FIRMWARE_CHUNK_SIZE);
+
+		ret = vnt_control_out(priv, 0, 0x1200 + ii, 0x0000, length,
+				      fw->data + ii);
+		if (ret)
+			goto free_fw;
+
+		dev_dbg(dev, "Download firmware...%d %zu\n", ii, fw->size);
+	}
+
+free_fw:
+	release_firmware(fw);
+end:
+	return ret;
+}
+
+static int vnt_firmware_branch_to_sram(struct vnt_private *priv)
+{
+	dev_dbg(&priv->usb->dev, "---->Branch to Sram\n");
+
+	return vnt_control_out(priv, 1, 0x1200, 0x0000, 0, NULL);
+}
+
+static int vnt_check_firmware_version(struct vnt_private *priv)
+{
+	int ret = 0;
+
+	ret = vnt_control_in(priv, MESSAGE_TYPE_READ, 0,
+			     MESSAGE_REQUEST_VERSION, 2,
+			     (u8 *)&priv->firmware_version);
+	if (ret) {
+		dev_dbg(&priv->usb->dev,
+			"Could not get firmware version: %d.\n", ret);
+		goto end;
+	}
+
+	dev_dbg(&priv->usb->dev, "Firmware Version [%04x]\n",
+		priv->firmware_version);
+
+	if (priv->firmware_version == 0xFFFF) {
+		dev_dbg(&priv->usb->dev, "In Loader.\n");
+		ret = -EINVAL;
+		goto end;
+	}
+
+	if (priv->firmware_version < FIRMWARE_VERSION) {
+		/* branch to loader for download new firmware */
+		ret = vnt_firmware_branch_to_sram(priv);
+		if (ret) {
+			dev_dbg(&priv->usb->dev,
+				"Could not branch to SRAM: %d.\n", ret);
+		} else {
+			ret = -EINVAL;
+		}
+	}
+
+end:
+	return ret;
+}
+
 /*
  * initialization of MAC & BBP registers
  */
@@ -146,8 +219,8 @@ static int vnt_init_registers(struct vnt_private *priv)
 	init_cmd->exist_sw_net_addr = priv->exist_sw_net_addr;
 	for (ii = 0; ii < ARRAY_SIZE(init_cmd->sw_net_addr); ii++)
 		init_cmd->sw_net_addr[ii] = priv->current_net_addr[ii];
-	init_cmd->short_retry_limit = priv->short_retry_limit;
-	init_cmd->long_retry_limit = priv->long_retry_limit;
+	init_cmd->short_retry_limit = priv->hw->wiphy->retry_short;
+	init_cmd->long_retry_limit = priv->hw->wiphy->retry_long;
 
 	/* issue card_init command to device */
 	ret = vnt_control_out(priv, MESSAGE_TYPE_CARDINIT, 0, 0,
@@ -324,19 +397,6 @@ static int vnt_init_registers(struct vnt_private *priv)
 	dev_dbg(&priv->usb->dev, "Network address = %pM\n",
 		priv->current_net_addr);
 
-	/*
-	 * set BB and packet type at the same time
-	 * set Short Slot Time, xIFS, and RSPINF
-	 */
-	if (priv->bb_type == BB_TYPE_11A)
-		priv->short_slot_time = true;
-	else
-		priv->short_slot_time = false;
-
-	ret = vnt_set_short_slot_time(priv);
-	if (ret)
-		goto end;
-
 	priv->radio_ctl = priv->eeprom[EEP_OFS_RADIOCTL];
 
 	if ((priv->radio_ctl & EEP_RADIOCTL_ENABLE) != 0) {
@@ -385,17 +445,13 @@ static void vnt_free_tx_bufs(struct vnt_private *priv)
 	struct vnt_usb_send_context *tx_context;
 	int ii;
 
+	usb_kill_anchored_urbs(&priv->tx_submitted);
+
 	for (ii = 0; ii < priv->num_tx_context; ii++) {
 		tx_context = priv->tx_context[ii];
 		if (!tx_context)
 			continue;
 
-		/* deallocate URBs */
-		if (tx_context->urb) {
-			usb_kill_urb(tx_context->urb);
-			usb_free_urb(tx_context->urb);
-		}
-
 		kfree(tx_context);
 	}
 }
@@ -436,6 +492,8 @@ static int vnt_alloc_bufs(struct vnt_private *priv)
 	struct vnt_rcb *rcb;
 	int ii;
 
+	init_usb_anchor(&priv->tx_submitted);
+
 	for (ii = 0; ii < priv->num_tx_context; ii++) {
 		tx_context = kmalloc(sizeof(*tx_context), GFP_KERNEL);
 		if (!tx_context) {
@@ -446,14 +504,6 @@ static int vnt_alloc_bufs(struct vnt_private *priv)
 		priv->tx_context[ii] = tx_context;
 		tx_context->priv = priv;
 		tx_context->pkt_no = ii;
-
-		/* allocate URBs */
-		tx_context->urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!tx_context->urb) {
-			ret = -ENOMEM;
-			goto free_tx;
-		}
-
 		tx_context->in_use = false;
 	}
 
@@ -683,15 +733,14 @@ static int vnt_config(struct ieee80211_hw *hw, u32 changed)
 			priv->bb_type = BB_TYPE_11G;
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		if (priv->bb_type == BB_TYPE_11B)
-			priv->current_rate = RATE_1M;
-		else
-			priv->current_rate = RATE_54M;
+	if (changed & IEEE80211_CONF_CHANGE_POWER)
+		vnt_rf_setpower(priv, conf->chandef.chan);
 
-		vnt_rf_setpower(priv, priv->current_rate,
-				conf->chandef.chan->hw_value);
-	}
+	if (conf->flags & (IEEE80211_CONF_OFFCHANNEL | IEEE80211_CONF_IDLE))
+		/* Set max sensitivity*/
+		vnt_update_pre_ed_threshold(priv, true);
+	else
+		vnt_update_pre_ed_threshold(priv, false);
 
 	return 0;
 }
@@ -718,10 +767,10 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		if (conf->use_short_preamble) {
 			vnt_mac_enable_barker_preamble_mode(priv);
-			priv->preamble_type = true;
+			priv->preamble_type = PREAMBLE_SHORT;
 		} else {
 			vnt_mac_disable_barker_preamble_mode(priv);
-			priv->preamble_type = false;
+			priv->preamble_type = PREAMBLE_LONG;
 		}
 	}
 
@@ -740,16 +789,14 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 
 		vnt_set_short_slot_time(priv);
 		vnt_set_vga_gain_offset(priv, priv->bb_vga[0]);
-		vnt_update_pre_ed_threshold(priv, false);
 	}
 
 	if (changed & (BSS_CHANGED_BASIC_RATES | BSS_CHANGED_ERP_PREAMBLE |
 		       BSS_CHANGED_ERP_SLOT))
 		vnt_set_bss_mode(priv);
 
-	if (changed & BSS_CHANGED_TXPOWER)
-		vnt_rf_setpower(priv, priv->current_rate,
-				conf->chandef.chan->hw_value);
+	if (changed & (BSS_CHANGED_TXPOWER | BSS_CHANGED_BANDWIDTH))
+		vnt_rf_setpower(priv, conf->chandef.chan);
 
 	if (changed & BSS_CHANGED_BEACON_ENABLED) {
 		dev_dbg(&priv->usb->dev,
@@ -767,10 +814,17 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_BEACON_INFO) &&
 	    priv->op_mode != NL80211_IFTYPE_AP) {
 		if (conf->assoc && conf->beacon_rate) {
+			u16 ps_beacon_int = conf->beacon_int;
+
+			if (conf->dtim_period)
+				ps_beacon_int *= conf->dtim_period;
+			else if (hw->conf.listen_interval)
+				ps_beacon_int *= hw->conf.listen_interval;
+
 			vnt_mac_reg_bits_on(priv, MAC_REG_TFTCTL,
 					    TFTCTL_TSFCNTREN);
 
-			vnt_mac_set_beacon_interval(priv, conf->beacon_int);
+			vnt_mac_set_beacon_interval(priv, ps_beacon_int);
 
 			vnt_reset_next_tbtt(priv, conf->beacon_int);
 
@@ -778,7 +832,7 @@ static void vnt_bss_info_changed(struct ieee80211_hw *hw,
 				       conf->sync_tsf, priv->current_tsf);
 
 			vnt_update_next_tbtt(priv,
-					     conf->sync_tsf, conf->beacon_int);
+					     conf->sync_tsf, ps_beacon_int);
 		} else {
 			vnt_clear_current_tsf(priv);
 
@@ -868,25 +922,6 @@ static int vnt_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return 0;
 }
 
-static void vnt_sw_scan_start(struct ieee80211_hw *hw,
-			      struct ieee80211_vif *vif,
-			      const u8 *addr)
-{
-	struct vnt_private *priv = hw->priv;
-
-	/* Set max sensitivity*/
-	vnt_update_pre_ed_threshold(priv, true);
-}
-
-static void vnt_sw_scan_complete(struct ieee80211_hw *hw,
-				 struct ieee80211_vif *vif)
-{
-	struct vnt_private *priv = hw->priv;
-
-	/* Return sensitivity to channel level*/
-	vnt_update_pre_ed_threshold(priv, false);
-}
-
 static int vnt_get_stats(struct ieee80211_hw *hw,
 			 struct ieee80211_low_level_stats *stats)
 {
@@ -932,8 +967,6 @@ static const struct ieee80211_ops vnt_mac_ops = {
 	.prepare_multicast	= vnt_prepare_multicast,
 	.configure_filter	= vnt_configure,
 	.set_key		= vnt_set_key,
-	.sw_scan_start		= vnt_sw_scan_start,
-	.sw_scan_complete	= vnt_sw_scan_complete,
 	.get_stats		= vnt_get_stats,
 	.get_tsf		= vnt_get_tsf,
 	.set_tsf		= vnt_set_tsf,
@@ -1010,6 +1043,8 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	ieee80211_hw_set(priv->hw, SUPPORTS_PS);
 	ieee80211_hw_set(priv->hw, PS_NULLFUNC_STACK);
 
+	priv->hw->extra_tx_headroom =
+		sizeof(struct vnt_tx_buffer) + sizeof(struct vnt_tx_usb_header);
 	priv->hw->max_signal = 100;
 
 	SET_IEEE80211_DEV(priv->hw, &intf->dev);
@@ -1078,3 +1113,5 @@ static struct usb_driver vt6656_driver = {
 };
 
 module_usb_driver(vt6656_driver);
+
+MODULE_FIRMWARE(FIRMWARE_NAME);
diff --git a/drivers/staging/vt6656/power.c b/drivers/staging/vt6656/power.c
index 7a086c72d5a8..2f49c870272a 100644
--- a/drivers/staging/vt6656/power.c
+++ b/drivers/staging/vt6656/power.c
@@ -63,41 +63,29 @@ void vnt_enable_power_saving(struct vnt_private *priv, u16 listen_interval)
 	 */
 	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_GO2DOZE);
 
-	if (listen_interval >= 2) {
-		/* clear always listen beacon */
-		vnt_mac_reg_bits_off(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
-
-		/* first time set listen next beacon */
-		vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_LNBCN);
-	} else {
-		/* always listen beacon */
-		vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
-	}
+	/* always listen beacon */
+	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
 
 	dev_dbg(&priv->usb->dev,  "PS:Power Saving Mode Enable...\n");
 }
 
-/*
- *
- * Routine Description:
- * Disable hw power saving functions
- *
- * Return Value:
- *    None.
- *
- */
-
-void vnt_disable_power_saving(struct vnt_private *priv)
+int vnt_disable_power_saving(struct vnt_private *priv)
 {
+	int ret;
+
 	/* disable power saving hw function */
-	vnt_control_out(priv, MESSAGE_TYPE_DISABLE_PS, 0,
-			0, 0, NULL);
+	ret = vnt_control_out(priv, MESSAGE_TYPE_DISABLE_PS, 0,
+			      0, 0, NULL);
+	if (ret)
+		return ret;
 
 	/* clear AutoSleep */
 	vnt_mac_reg_bits_off(priv, MAC_REG_PSCFG, PSCFG_AUTOSLEEP);
 
 	/* set always listen beacon */
 	vnt_mac_reg_bits_on(priv, MAC_REG_PSCTL, PSCTL_ALBCN);
+
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/vt6656/power.h b/drivers/staging/vt6656/power.h
index 58755ae16e5a..160872026db3 100644
--- a/drivers/staging/vt6656/power.h
+++ b/drivers/staging/vt6656/power.h
@@ -18,7 +18,7 @@
 
 #define C_PWBT	1000 /* micro sec. power up before TBTT */
 
-void vnt_disable_power_saving(struct vnt_private *priv);
+int vnt_disable_power_saving(struct vnt_private *priv);
 void vnt_enable_power_saving(struct vnt_private *priv, u16 listen_interval);
 int vnt_next_tbtt_wakeup(struct vnt_private *priv);
 
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index 43237b7e1dbe..5b8da06e3916 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -21,22 +21,16 @@
  *
  */
 
+#include <linux/errno.h>
 #include "mac.h"
 #include "rf.h"
 #include "baseband.h"
 #include "usbpipe.h"
 
 #define CB_AL2230_INIT_SEQ    15
-#define AL2230_PWR_IDX_LEN    64
-
 #define CB_AL7230_INIT_SEQ    16
-#define AL7230_PWR_IDX_LEN    64
-
 #define CB_VT3226_INIT_SEQ    11
-#define VT3226_PWR_IDX_LEN    64
-
 #define CB_VT3342_INIT_SEQ    13
-#define VT3342_PWR_IDX_LEN    64
 
 static u8 al2230_init_table[CB_AL2230_INIT_SEQ][3] = {
 	{0x03, 0xf7, 0x90},
@@ -518,72 +512,45 @@ static u8 vt3342_channel_table1[CB_MAX_CHANNEL][3] = {
 	{0x03, 0x00, 0x04}
 };
 
-/* Power Table */
-static const u32 al2230_power_table[AL2230_PWR_IDX_LEN] = {
-	0x04040900,
-	0x04041900,
-	0x04042900,
-	0x04043900,
-	0x04044900,
-	0x04045900,
-	0x04046900,
-	0x04047900,
-	0x04048900,
-	0x04049900,
-	0x0404a900,
-	0x0404b900,
-	0x0404c900,
-	0x0404d900,
-	0x0404e900,
-	0x0404f900,
-	0x04050900,
-	0x04051900,
-	0x04052900,
-	0x04053900,
-	0x04054900,
-	0x04055900,
-	0x04056900,
-	0x04057900,
-	0x04058900,
-	0x04059900,
-	0x0405a900,
-	0x0405b900,
-	0x0405c900,
-	0x0405d900,
-	0x0405e900,
-	0x0405f900,
-	0x04060900,
-	0x04061900,
-	0x04062900,
-	0x04063900,
-	0x04064900,
-	0x04065900,
-	0x04066900,
-	0x04067900,
-	0x04068900,
-	0x04069900,
-	0x0406a900,
-	0x0406b900,
-	0x0406c900,
-	0x0406d900,
-	0x0406e900,
-	0x0406f900,
-	0x04070900,
-	0x04071900,
-	0x04072900,
-	0x04073900,
-	0x04074900,
-	0x04075900,
-	0x04076900,
-	0x04077900,
-	0x04078900,
-	0x04079900,
-	0x0407a900,
-	0x0407b900,
-	0x0407c900,
-	0x0407d900,
-	0x0407e900,
-	0x0407f900
+enum {
+	VNT_TABLE_INIT = 0,
+	VNT_TABLE_INIT_2 = 0,
+	VNT_TABLE_0 = 1,
+	VNT_TABLE_1 = 2,
+	VNT_TABLE_2 = 1
+};
+
+struct vnt_table_info {
+	u8 *addr;
+	int length;
+};
+
+static const struct vnt_table_info vnt_table_seq[][3] = {
+	{	/* RF_AL2230, RF_AL2230S init table, channel table 0 and 1 */
+		{&al2230_init_table[0][0], CB_AL2230_INIT_SEQ * 3},
+		{&al2230_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&al2230_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}, {	/* RF_AIROHA7230 init table, channel table 0 and 1 */
+		{&al7230_init_table[0][0], CB_AL7230_INIT_SEQ * 3},
+		{&al7230_channel_table0[0][0], CB_MAX_CHANNEL * 3},
+		{&al7230_channel_table1[0][0], CB_MAX_CHANNEL * 3}
+	}, {	/* RF_VT3226 init table, channel table 0 and 1 */
+		{&vt3226_init_table[0][0], CB_VT3226_INIT_SEQ * 3},
+		{&vt3226_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&vt3226_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}, {	/* RF_VT3226D0 init table, channel table 0 and 1 */
+		{&vt3226d0_init_table[0][0], CB_VT3226_INIT_SEQ * 3},
+		{&vt3226_channel_table0[0][0], CB_MAX_CHANNEL_24G * 3},
+		{&vt3226_channel_table1[0][0], CB_MAX_CHANNEL_24G * 3}
+	}, {	/* RF_VT3342A0 init table, channel table 0 and 1 */
+		{&vt3342a0_init_table[0][0], CB_VT3342_INIT_SEQ * 3},
+		{&vt3342_channel_table0[0][0], CB_MAX_CHANNEL * 3},
+		{&vt3342_channel_table1[0][0], CB_MAX_CHANNEL * 3}
+	}, {	/* RF_AIROHA7230 init table 2 and channel table 2 */
+		{&al7230_init_table_amode[0][0], CB_AL7230_INIT_SEQ * 3},
+		{&al7230_channel_table2[0][0], CB_MAX_CHANNEL * 3},
+		{NULL, 0}
+	}
 };
 
 /*
@@ -600,124 +567,90 @@ int vnt_rf_write_embedded(struct vnt_private *priv, u32 data)
 	reg_data[2] = (u8)(data >> 16);
 	reg_data[3] = (u8)(data >> 24);
 
-	vnt_control_out(priv, MESSAGE_TYPE_WRITE_IFRF,
-			0, 0, ARRAY_SIZE(reg_data), reg_data);
-
-	return true;
-}
-
-/* Set Tx power by rate and channel number */
-int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
-{
-	u8 power = priv->cck_pwr;
-
-	if (channel == 0)
-		return -EINVAL;
-
-	switch (rate) {
-	case RATE_1M:
-	case RATE_2M:
-	case RATE_5M:
-	case RATE_11M:
-		channel--;
-
-		if (channel < sizeof(priv->cck_pwr_tbl))
-			power = priv->cck_pwr_tbl[channel];
-		break;
-	case RATE_6M:
-	case RATE_9M:
-	case RATE_12M:
-	case RATE_18M:
-	case RATE_24M:
-	case RATE_36M:
-	case RATE_48M:
-	case RATE_54M:
-		if (channel > CB_MAX_CHANNEL_24G)
-			power = priv->ofdm_a_pwr_tbl[channel - 15];
-		else
-			power = priv->ofdm_pwr_tbl[channel - 1];
-		break;
-	}
-
-	return vnt_rf_set_txpower(priv, power, rate);
+	return vnt_control_out(priv, MESSAGE_TYPE_WRITE_IFRF, 0, 0,
+			       ARRAY_SIZE(reg_data), reg_data);
 }
 
 static u8 vnt_rf_addpower(struct vnt_private *priv)
 {
+	int base;
 	s32 rssi = -priv->current_rssi;
 
 	if (!rssi)
 		return 7;
 
-	if (priv->rf_type == RF_VT3226D0) {
-		if (rssi < -70)
-			return 9;
-		else if (rssi < -65)
-			return 7;
-		else if (rssi < -60)
-			return 5;
-	} else {
-		if (rssi < -80)
-			return 9;
-		else if (rssi < -75)
-			return 7;
-		else if (rssi < -70)
-			return 5;
-	}
+	if (priv->rf_type == RF_VT3226D0)
+		base = -60;
+	else
+		base = -70;
+
+	if (rssi < base)
+		return ((rssi - base + 1) / -5) * 2 + 5;
 
 	return 0;
 }
 
 /* Set Tx power by power level and rate */
-int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
+static int vnt_rf_set_txpower(struct vnt_private *priv, u8 power,
+			      struct ieee80211_channel *ch)
 {
 	u32 power_setting = 0;
-	int ret = true;
+	int ret = 0;
 
 	power += vnt_rf_addpower(priv);
 	if (power > VNT_RF_MAX_POWER)
 		power = VNT_RF_MAX_POWER;
 
 	if (priv->power == power)
-		return true;
+		return 0;
 
 	priv->power = power;
 
 	switch (priv->rf_type) {
 	case RF_AL2230:
-		if (power >= AL2230_PWR_IDX_LEN)
-			return false;
+		power_setting = 0x0404090 | (power << 12);
 
-		ret &= vnt_rf_write_embedded(priv, al2230_power_table[power]);
+		ret = vnt_rf_write_embedded(priv, power_setting);
+		if (ret)
+			return ret;
 
-		if (rate <= RATE_11M)
-			ret &= vnt_rf_write_embedded(priv, 0x0001b400);
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM)
+			ret = vnt_rf_write_embedded(priv, 0x0001b400);
 		else
-			ret &= vnt_rf_write_embedded(priv, 0x0005a400);
+			ret = vnt_rf_write_embedded(priv, 0x0005a400);
+
 		break;
 	case RF_AL2230S:
-		if (power >= AL2230_PWR_IDX_LEN)
-			return false;
+		power_setting = 0x0404090 | (power << 12);
 
-		ret &= vnt_rf_write_embedded(priv, al2230_power_table[power]);
+		ret = vnt_rf_write_embedded(priv, power_setting);
+		if (ret)
+			return ret;
 
-		if (rate <= RATE_11M) {
-			ret &= vnt_rf_write_embedded(priv, 0x040c1400);
-			ret &= vnt_rf_write_embedded(priv, 0x00299b00);
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+			ret = vnt_rf_write_embedded(priv, 0x040c1400);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00299b00);
 		} else {
-			ret &= vnt_rf_write_embedded(priv, 0x0005a400);
-			ret &= vnt_rf_write_embedded(priv, 0x00099b00);
+			ret = vnt_rf_write_embedded(priv, 0x0005a400);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00099b00);
 		}
+
 		break;
 
 	case RF_AIROHA7230:
-		if (rate <= RATE_11M)
-			ret &= vnt_rf_write_embedded(priv, 0x111bb900);
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM)
+			ret = vnt_rf_write_embedded(priv, 0x111bb900);
 		else
-			ret &= vnt_rf_write_embedded(priv, 0x221bb900);
+			ret = vnt_rf_write_embedded(priv, 0x221bb900);
 
-		if (power >= AL7230_PWR_IDX_LEN)
-			return false;
+		if (ret)
+			return ret;
 
 		/*
 		 * 0x080F1B00 for 3 wire control TxGain(D10)
@@ -725,61 +658,68 @@ int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
 		 */
 		power_setting = 0x080c0b00 | (power << 12);
 
-		ret &= vnt_rf_write_embedded(priv, power_setting);
-
+		ret = vnt_rf_write_embedded(priv, power_setting);
 		break;
 
 	case RF_VT3226:
-		if (power >= VT3226_PWR_IDX_LEN)
-			return false;
 		power_setting = ((0x3f - power) << 20) | (0x17 << 8);
 
-		ret &= vnt_rf_write_embedded(priv, power_setting);
-
+		ret = vnt_rf_write_embedded(priv, power_setting);
 		break;
 	case RF_VT3226D0:
-		if (power >= VT3226_PWR_IDX_LEN)
-			return false;
-
-		if (rate <= RATE_11M) {
-			u16 hw_value = priv->hw->conf.chandef.chan->hw_value;
+		if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+			u16 hw_value = ch->hw_value;
 
 			power_setting = ((0x3f - power) << 20) | (0xe07 << 8);
 
-			ret &= vnt_rf_write_embedded(priv, power_setting);
-			ret &= vnt_rf_write_embedded(priv, 0x03c6a200);
+			ret = vnt_rf_write_embedded(priv, power_setting);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x03c6a200);
+			if (ret)
+				return ret;
 
 			dev_dbg(&priv->usb->dev,
 				"%s 11b channel [%d]\n", __func__, hw_value);
 
 			hw_value--;
 
-			if (hw_value < ARRAY_SIZE(vt3226d0_lo_current_table))
-				ret &= vnt_rf_write_embedded(priv,
+			if (hw_value < ARRAY_SIZE(vt3226d0_lo_current_table)) {
+				ret = vnt_rf_write_embedded(priv,
 					vt3226d0_lo_current_table[hw_value]);
+				if (ret)
+					return ret;
+			}
 
-			ret &= vnt_rf_write_embedded(priv, 0x015C0800);
+			ret = vnt_rf_write_embedded(priv, 0x015C0800);
 		} else {
 			dev_dbg(&priv->usb->dev,
 				"@@@@ %s> 11G mode\n", __func__);
 
 			power_setting = ((0x3f - power) << 20) | (0x7 << 8);
 
-			ret &= vnt_rf_write_embedded(priv, power_setting);
-			ret &= vnt_rf_write_embedded(priv, 0x00C6A200);
-			ret &= vnt_rf_write_embedded(priv, 0x016BC600);
-			ret &= vnt_rf_write_embedded(priv, 0x00900800);
+			ret = vnt_rf_write_embedded(priv, power_setting);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00C6A200);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x016BC600);
+			if (ret)
+				return ret;
+
+			ret = vnt_rf_write_embedded(priv, 0x00900800);
 		}
+
 		break;
 
 	case RF_VT3342A0:
-		if (power >= VT3342_PWR_IDX_LEN)
-			return false;
-
 		power_setting =  ((0x3f - power) << 20) | (0x27 << 8);
 
-		ret &= vnt_rf_write_embedded(priv, power_setting);
-
+		ret = vnt_rf_write_embedded(priv, power_setting);
 		break;
 	default:
 		break;
@@ -787,6 +727,36 @@ int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate)
 	return ret;
 }
 
+/* Set Tx power by channel number type */
+int vnt_rf_setpower(struct vnt_private *priv,
+		    struct ieee80211_channel *ch)
+{
+	u16 channel;
+	u8 power = priv->cck_pwr;
+
+	if (!ch)
+		return -EINVAL;
+
+	/* set channel number to array number */
+	channel = ch->hw_value - 1;
+
+	if (ch->flags & IEEE80211_CHAN_NO_OFDM) {
+		if (channel < ARRAY_SIZE(priv->cck_pwr_tbl))
+			power = priv->cck_pwr_tbl[channel];
+	} else if (ch->band == NL80211_BAND_5GHZ) {
+		/* remove 14 channels to array size */
+		channel -= 14;
+
+		if (channel < ARRAY_SIZE(priv->ofdm_a_pwr_tbl))
+			power = priv->ofdm_a_pwr_tbl[channel];
+	} else {
+		if (channel < ARRAY_SIZE(priv->ofdm_pwr_tbl))
+			power = priv->ofdm_pwr_tbl[channel];
+	}
+
+	return vnt_rf_set_txpower(priv, power, ch);
+}
+
 /* Convert rssi to dbm */
 void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm)
 {
@@ -813,140 +783,73 @@ void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm)
 
 int vnt_rf_table_download(struct vnt_private *priv)
 {
-	int ret = 0;
-	u16 length1 = 0, length2 = 0, length3 = 0;
-	u8 *addr1 = NULL, *addr2 = NULL, *addr3 = NULL;
-	u16 length, value;
-	u8 array[256];
+	int ret;
+	int idx = -1;
+	const struct vnt_table_info *table_seq;
 
 	switch (priv->rf_type) {
 	case RF_AL2230:
 	case RF_AL2230S:
-		length1 = CB_AL2230_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL_24G * 3;
-		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &al2230_init_table[0][0];
-		addr2 = &al2230_channel_table0[0][0];
-		addr3 = &al2230_channel_table1[0][0];
+		idx = 0;
 		break;
 	case RF_AIROHA7230:
-		length1 = CB_AL7230_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL * 3;
-		length3 = CB_MAX_CHANNEL * 3;
-		addr1 = &al7230_init_table[0][0];
-		addr2 = &al7230_channel_table0[0][0];
-		addr3 = &al7230_channel_table1[0][0];
+		idx = 1;
 		break;
 	case RF_VT3226:
-		length1 = CB_VT3226_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL_24G * 3;
-		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &vt3226_init_table[0][0];
-		addr2 = &vt3226_channel_table0[0][0];
-		addr3 = &vt3226_channel_table1[0][0];
+		idx = 2;
 		break;
 	case RF_VT3226D0:
-		length1 = CB_VT3226_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL_24G * 3;
-		length3 = CB_MAX_CHANNEL_24G * 3;
-		addr1 = &vt3226d0_init_table[0][0];
-		addr2 = &vt3226_channel_table0[0][0];
-		addr3 = &vt3226_channel_table1[0][0];
+		idx = 3;
 		break;
 	case RF_VT3342A0:
-		length1 = CB_VT3342_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL * 3;
-		length3 = CB_MAX_CHANNEL * 3;
-		addr1 = &vt3342a0_init_table[0][0];
-		addr2 = &vt3342_channel_table0[0][0];
-		addr3 = &vt3342_channel_table1[0][0];
+		idx = 4;
 		break;
 	}
 
-	/* Init Table */
-	memcpy(array, addr1, length1);
+	if (idx < 0)
+		return 0;
 
+	table_seq = &vnt_table_seq[idx][0];
+
+	/* Init Table */
 	ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
-			      MESSAGE_REQUEST_RF_INIT, length1, array);
+			      MESSAGE_REQUEST_RF_INIT,
+			      table_seq[VNT_TABLE_INIT].length,
+			      table_seq[VNT_TABLE_INIT].addr);
 	if (ret)
-		goto end;
+		return ret;
 
 	/* Channel Table 0 */
-	value = 0;
-	while (length2 > 0) {
-		if (length2 >= 64)
-			length = 64;
-		else
-			length = length2;
-
-		memcpy(array, addr2, length);
-
-		ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, value,
-				      MESSAGE_REQUEST_RF_CH0, length, array);
-		if (ret)
-			goto end;
-
-		length2 -= length;
-		value += length;
-		addr2 += length;
-	}
-
-	/* Channel table 1 */
-	value = 0;
-	while (length3 > 0) {
-		if (length3 >= 64)
-			length = 64;
-		else
-			length = length3;
-
-		memcpy(array, addr3, length);
+	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+				     MESSAGE_REQUEST_RF_CH0,
+				     table_seq[VNT_TABLE_0].length,
+				     table_seq[VNT_TABLE_0].addr);
+	if (ret)
+		return ret;
 
-		ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, value,
-				      MESSAGE_REQUEST_RF_CH1, length, array);
-		if (ret)
-			goto end;
-
-		length3 -= length;
-		value += length;
-		addr3 += length;
-	}
+	/* Channel Table 1 */
+	ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+				     MESSAGE_REQUEST_RF_CH1,
+				     table_seq[VNT_TABLE_1].length,
+				     table_seq[VNT_TABLE_1].addr);
 
 	if (priv->rf_type == RF_AIROHA7230) {
-		length1 = CB_AL7230_INIT_SEQ * 3;
-		length2 = CB_MAX_CHANNEL * 3;
-		addr1 = &al7230_init_table_amode[0][0];
-		addr2 = &al7230_channel_table2[0][0];
-
-		memcpy(array, addr1, length1);
+		table_seq = &vnt_table_seq[5][0];
 
 		/* Init Table 2 */
 		ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, 0,
-				      MESSAGE_REQUEST_RF_INIT2, length1, array);
+				      MESSAGE_REQUEST_RF_INIT2,
+				      table_seq[VNT_TABLE_INIT_2].length,
+				      table_seq[VNT_TABLE_INIT_2].addr);
 		if (ret)
-			goto end;
+			return ret;
 
-		/* Channel Table 0 */
-		value = 0;
-		while (length2 > 0) {
-			if (length2 >= 64)
-				length = 64;
-			else
-				length = length2;
-
-			memcpy(array, addr2, length);
-
-			ret = vnt_control_out(priv, MESSAGE_TYPE_WRITE, value,
-					      MESSAGE_REQUEST_RF_CH2, length,
-					      array);
-			if (ret)
-				goto end;
-
-			length2 -= length;
-			value += length;
-			addr2 += length;
-		}
+		/* Channel Table 2 */
+		ret = vnt_control_out_blocks(priv, VNT_REG_BLOCK_SIZE,
+					     MESSAGE_REQUEST_RF_CH2,
+					     table_seq[VNT_TABLE_2].length,
+					     table_seq[VNT_TABLE_2].addr);
 	}
 
-end:
 	return ret;
 }
diff --git a/drivers/staging/vt6656/rf.h b/drivers/staging/vt6656/rf.h
index 7494546d71b8..493faaf4e2b5 100644
--- a/drivers/staging/vt6656/rf.h
+++ b/drivers/staging/vt6656/rf.h
@@ -41,8 +41,7 @@
 #define	VNT_RF_REG_LEN      0x17 /* 24 bit length */
 
 int vnt_rf_write_embedded(struct vnt_private *priv, u32 data);
-int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel);
-int vnt_rf_set_txpower(struct vnt_private *priv, u8 power, u32 rate);
+int vnt_rf_setpower(struct vnt_private *priv, struct ieee80211_channel *ch);
 void vnt_rf_rssi_to_dbm(struct vnt_private *priv, u8 rssi, long *dbm);
 int vnt_rf_table_download(struct vnt_private *priv);
 
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 9439d190f431..5dd6b4d2bf20 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -13,8 +13,6 @@
  *
  * Functions:
  *      vnt_generate_tx_parameter - Generate tx dma required parameter.
- *      vnt_get_rtscts_duration_le- get rtx/cts required duration
- *      vnt_get_rtscts_rsvtime_le- get rts/cts reserved time
  *      vnt_get_rsvtime- get frame reserved time
  *      vnt_fill_cts_head- fulfill CTS ctl header
  *
@@ -38,13 +36,24 @@ static const u16 vnt_time_stampoff[2][MAX_RATE] = {
 	{384, 192, 130, 113, 54, 43, 37, 31, 28, 25, 24, 23},
 };
 
-#define RTSDUR_BB       0
-#define RTSDUR_BA       1
-#define RTSDUR_AA       2
-#define CTSDUR_BA       3
 #define DATADUR_B       10
 #define DATADUR_A       11
 
+static const u8 vnt_phy_signal[] = {
+	0x00,	/* RATE_1M  */
+	0x01,	/* RATE_2M  */
+	0x02,	/* RATE_5M  */
+	0x03,	/* RATE_11M */
+	0x8b,	/* RATE_6M  */
+	0x8f,	/* RATE_9M  */
+	0x8a,	/* RATE_12M */
+	0x8e,	/* RATE_18M */
+	0x89,	/* RATE_24M */
+	0x8d,	/* RATE_36M */
+	0x88,	/* RATE_48M */
+	0x8c	/* RATE_54M */
+};
+
 static struct vnt_usb_send_context
 	*vnt_get_free_context(struct vnt_private *priv)
 {
@@ -60,11 +69,6 @@ static struct vnt_usb_send_context
 		context = priv->tx_context[ii];
 		if (!context->in_use) {
 			context->in_use = true;
-			memset(context->data, 0,
-			       MAX_TOTAL_SIZE_WITH_ALL_HEADERS);
-
-			context->hdr = NULL;
-
 			return context;
 		}
 	}
@@ -78,146 +82,105 @@ static struct vnt_usb_send_context
 	return NULL;
 }
 
-static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
+/* Get Length, Service, and Signal fields of Phy for Tx */
+static void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+			      u16 tx_rate, u8 pkt_type,
+			      struct vnt_phy_field *phy)
 {
-	return cpu_to_le16(vnt_time_stampoff[priv->preamble_type % 2]
-							[rate % MAX_RATE]);
-}
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	int i;
+	u8 mask = 0;
+	u8 preamble_type = priv->preamble_type;
+
+	bit_count = frame_length * 8;
+	ext_bit = false;
+
+	switch (tx_rate) {
+	case RATE_1M:
+		count = bit_count;
+		break;
+	case RATE_2M:
+		count = bit_count / 2;
+		break;
+	case RATE_5M:
+		count = DIV_ROUND_UP(bit_count * 10, 55);
+		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
 
-static u32 vnt_get_rsvtime(struct vnt_private *priv, u8 pkt_type,
-			   u32 frame_length, u16 rate, int need_ack)
-{
-	u32 data_time, ack_time;
+		if (tmp != bit_count) {
+			count++;
 
-	data_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-				       frame_length, rate);
+			if ((bit_count - tmp) <= 3)
+				ext_bit = true;
+		}
+
+		break;
+	}
 
-	if (pkt_type == PK_TYPE_11B)
-		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type, 14,
-					      (u16)priv->top_cck_basic_rate);
-	else
-		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type, 14,
-					      (u16)priv->top_ofdm_basic_rate);
+	if (tx_rate > RATE_11M) {
+		if (pkt_type == PK_TYPE_11A)
+			mask = BIT(4);
+	} else if (tx_rate > RATE_1M) {
+		if (preamble_type == PREAMBLE_SHORT)
+			mask = BIT(3);
+	}
 
-	if (need_ack)
-		return data_time + priv->sifs + ack_time;
+	i = tx_rate > RATE_54M ? RATE_54M : tx_rate;
+	phy->signal = vnt_phy_signal[i] | mask;
+	phy->service = 0x00;
 
-	return data_time;
+	if (pkt_type == PK_TYPE_11B) {
+		if (ext_bit)
+			phy->service |= 0x80;
+		phy->len = cpu_to_le16((u16)count);
+	} else {
+		phy->len = cpu_to_le16((u16)frame_length);
+	}
 }
 
-static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
-				    u32 frame_length, u16 rate, int need_ack)
+static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
 {
-	return cpu_to_le16((u16)vnt_get_rsvtime(priv, pkt_type,
-		frame_length, rate, need_ack));
+	return cpu_to_le16(vnt_time_stampoff[priv->preamble_type % 2]
+							[rate % MAX_RATE]);
 }
 
-static __le16 vnt_get_rtscts_rsvtime_le(struct vnt_private *priv, u8 rsv_type,
-					u8 pkt_type, u32 frame_length,
-					u16 current_rate)
+static __le16 vnt_rxtx_rsvtime_le16(struct vnt_usb_send_context *context)
 {
-	u32 rrv_time, rts_time, cts_time, ack_time, data_time;
-
-	rrv_time = 0;
-	rts_time = 0;
-	cts_time = 0;
-	ack_time = 0;
-
-	data_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-				       frame_length, current_rate);
-
-	if (rsv_type == 0) {
-		rts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-					      20, priv->top_cck_basic_rate);
-		ack_time = vnt_get_frame_time(priv->preamble_type,
-					      pkt_type, 14,
-					      priv->top_cck_basic_rate);
-		cts_time = ack_time;
-
-	} else if (rsv_type == 1) {
-		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);
-		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-					      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);
-		ack_time = vnt_get_frame_time(priv->preamble_type,
-					      pkt_type, 14,
-					      priv->top_ofdm_basic_rate);
-		cts_time = ack_time;
-
-	} else if (rsv_type == 3) {
-		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-					      14, priv->top_cck_basic_rate);
-		ack_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-					      14, priv->top_ofdm_basic_rate);
-
-		rrv_time = cts_time + ack_time + data_time + 2 * priv->sifs;
-
-		return cpu_to_le16((u16)rrv_time);
-	}
-
-	rrv_time = rts_time + cts_time + ack_time + data_time + 3 * priv->sifs;
+	struct vnt_private *priv = context->priv;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
+	struct ieee80211_rate *rate = ieee80211_get_tx_rate(priv->hw, info);
 
-	return cpu_to_le16((u16)rrv_time);
+	return ieee80211_generic_frame_duration(priv->hw,
+						 info->control.vif, info->band,
+						 context->frame_len,
+						 rate);
 }
 
-static __le16 vnt_get_rtscts_duration_le(struct vnt_usb_send_context *context,
-					 u8 dur_type, u8 pkt_type, u16 rate)
+static __le16 vnt_get_rts_duration(struct vnt_usb_send_context *context)
 {
 	struct vnt_private *priv = context->priv;
-	u32 cts_time = 0, dur_time = 0;
-	u32 frame_length = context->frame_len;
-	u8 need_ack = context->need_ack;
-
-	switch (dur_type) {
-	case RTSDUR_BB:
-	case RTSDUR_BA:
-		cts_time = vnt_get_frame_time(priv->preamble_type, pkt_type,
-					      14, priv->top_cck_basic_rate);
-		dur_time = cts_time + 2 * priv->sifs +
-			vnt_get_rsvtime(priv, pkt_type,
-					frame_length, rate, need_ack);
-		break;
-
-	case RTSDUR_AA:
-		cts_time = vnt_get_frame_time(priv->preamble_type, 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);
-		break;
-
-	case CTSDUR_BA:
-		dur_time = priv->sifs + vnt_get_rsvtime(priv,
-				pkt_type, frame_length, rate, need_ack);
-		break;
-
-	default:
-		break;
-	}
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
 
-	return cpu_to_le16((u16)dur_time);
+	return ieee80211_rts_duration(priv->hw, priv->vif,
+				      context->frame_len, info);
 }
 
-static u16 vnt_mac_hdr_pos(struct vnt_usb_send_context *tx_context,
-			   struct ieee80211_hdr *hdr)
+static __le16 vnt_get_cts_duration(struct vnt_usb_send_context *context)
 {
-	u8 *head = tx_context->data + offsetof(struct vnt_tx_buffer, fifo_head);
-	u8 *hdr_pos = (u8 *)hdr;
-
-	tx_context->hdr = hdr;
-	if (!tx_context->hdr)
-		return 0;
+	struct vnt_private *priv = context->priv;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(context->skb);
 
-	return (u16)(hdr_pos - head);
+	return ieee80211_ctstoself_duration(priv->hw, priv->vif,
+					    context->frame_len, info);
 }
 
-static u16 vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
-			       struct vnt_tx_datahead_g *buf)
+static void vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
+				struct vnt_tx_datahead_g *buf)
 {
 	struct vnt_private *priv = tx_context->priv;
 	struct ieee80211_hdr *hdr =
@@ -236,14 +199,10 @@ static u16 vnt_rxtx_datahead_g(struct vnt_usb_send_context *tx_context,
 	buf->time_stamp_off_a = vnt_time_stamp_off(priv, rate);
 	buf->time_stamp_off_b = vnt_time_stamp_off(priv,
 						   priv->top_cck_basic_rate);
-
-	tx_context->tx_hdr_size = vnt_mac_hdr_pos(tx_context, &buf->hdr);
-
-	return le16_to_cpu(buf->duration_a);
 }
 
-static u16 vnt_rxtx_datahead_ab(struct vnt_usb_send_context *tx_context,
-				struct vnt_tx_datahead_ab *buf)
+static void vnt_rxtx_datahead_ab(struct vnt_usb_send_context *tx_context,
+				 struct vnt_tx_datahead_ab *buf)
 {
 	struct vnt_private *priv = tx_context->priv;
 	struct ieee80211_hdr *hdr =
@@ -258,14 +217,10 @@ static u16 vnt_rxtx_datahead_ab(struct vnt_usb_send_context *tx_context,
 	/* Get Duration and TimeStampOff */
 	buf->duration = hdr->duration_id;
 	buf->time_stamp_off = vnt_time_stamp_off(priv, rate);
-
-	tx_context->tx_hdr_size = vnt_mac_hdr_pos(tx_context, &buf->hdr);
-
-	return le16_to_cpu(buf->duration);
 }
 
-static int vnt_fill_ieee80211_rts(struct vnt_usb_send_context *tx_context,
-				  struct ieee80211_rts *rts, __le16 duration)
+static void vnt_fill_ieee80211_rts(struct vnt_usb_send_context *tx_context,
+				   struct ieee80211_rts *rts, __le16 duration)
 {
 	struct ieee80211_hdr *hdr =
 				(struct ieee80211_hdr *)tx_context->skb->data;
@@ -276,72 +231,56 @@ static int vnt_fill_ieee80211_rts(struct vnt_usb_send_context *tx_context,
 
 	ether_addr_copy(rts->ra, hdr->addr1);
 	ether_addr_copy(rts->ta, hdr->addr2);
-
-	return 0;
 }
 
-static u16 vnt_rxtx_rts_g_head(struct vnt_usb_send_context *tx_context,
-			       struct vnt_rts_g *buf)
+static void vnt_rxtx_rts_g_head(struct vnt_usb_send_context *tx_context,
+				struct vnt_rts_g *buf)
 {
 	struct vnt_private *priv = tx_context->priv;
 	u16 rts_frame_len = 20;
-	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);
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
 			  tx_context->pkt_type, &buf->a);
 
-	buf->duration_bb = vnt_get_rtscts_duration_le(tx_context, RTSDUR_BB,
-						      PK_TYPE_11B,
-						      priv->top_cck_basic_rate);
-	buf->duration_aa = vnt_get_rtscts_duration_le(tx_context, RTSDUR_AA,
-						      tx_context->pkt_type,
-						      current_rate);
-	buf->duration_ba = vnt_get_rtscts_duration_le(tx_context, RTSDUR_BA,
-						      tx_context->pkt_type,
-						      current_rate);
+	buf->duration_bb = vnt_get_rts_duration(tx_context);
+	buf->duration_aa = buf->duration_bb;
+	buf->duration_ba = buf->duration_bb;
 
 	vnt_fill_ieee80211_rts(tx_context, &buf->data, buf->duration_aa);
 
-	return vnt_rxtx_datahead_g(tx_context, &buf->data_head);
+	vnt_rxtx_datahead_g(tx_context, &buf->data_head);
 }
 
-static u16 vnt_rxtx_rts_ab_head(struct vnt_usb_send_context *tx_context,
-				struct vnt_rts_ab *buf)
+static void vnt_rxtx_rts_ab_head(struct vnt_usb_send_context *tx_context,
+				 struct vnt_rts_ab *buf)
 {
 	struct vnt_private *priv = tx_context->priv;
-	u16 current_rate = tx_context->tx_rate;
 	u16 rts_frame_len = 20;
 
 	vnt_get_phy_field(priv, rts_frame_len, priv->top_ofdm_basic_rate,
 			  tx_context->pkt_type, &buf->ab);
 
-	buf->duration = vnt_get_rtscts_duration_le(tx_context, RTSDUR_AA,
-						   tx_context->pkt_type,
-						   current_rate);
+	buf->duration = vnt_get_rts_duration(tx_context);
 
 	vnt_fill_ieee80211_rts(tx_context, &buf->data, buf->duration);
 
-	return vnt_rxtx_datahead_ab(tx_context, &buf->data_head);
+	vnt_rxtx_datahead_ab(tx_context, &buf->data_head);
 }
 
-static u16 vnt_fill_cts_head(struct vnt_usb_send_context *tx_context,
-			     union vnt_tx_data_head *head)
+static void vnt_fill_cts_head(struct vnt_usb_send_context *tx_context,
+			      union vnt_tx_data_head *head)
 {
 	struct vnt_private *priv = tx_context->priv;
 	struct vnt_cts *buf = &head->cts_g;
 	u32 cts_frame_len = 14;
-	u16 current_rate = tx_context->tx_rate;
 
 	/* Get SignalField,ServiceField,Length */
 	vnt_get_phy_field(priv, cts_frame_len, priv->top_cck_basic_rate,
 			  PK_TYPE_11B, &buf->b);
 	/* Get CTSDuration_ba */
-	buf->duration_ba =
-		vnt_get_rtscts_duration_le(tx_context, CTSDUR_BA,
-					   tx_context->pkt_type,
-					   current_rate);
+	buf->duration_ba = vnt_get_cts_duration(tx_context);
 	/*Get CTS Frame body*/
 	buf->data.duration = buf->duration_ba;
 	buf->data.frame_control =
@@ -349,127 +288,19 @@ static u16 vnt_fill_cts_head(struct vnt_usb_send_context *tx_context,
 
 	ether_addr_copy(buf->data.ra, priv->current_net_addr);
 
-	return vnt_rxtx_datahead_g(tx_context, &buf->data_head);
+	vnt_rxtx_datahead_g(tx_context, &buf->data_head);
 }
 
-static u16 vnt_rxtx_rts(struct vnt_usb_send_context *tx_context,
-			union vnt_tx_head *tx_head, bool need_mic)
+/* returns true if mic_hdr is needed */
+static bool vnt_fill_txkey(struct vnt_tx_buffer *tx_buffer, struct sk_buff *skb)
 {
-	struct vnt_private *priv = tx_context->priv;
-	struct vnt_rrv_time_rts *buf = &tx_head->tx_rts.rts;
-	union vnt_tx_data_head *head = &tx_head->tx_rts.tx.head;
-	u32 frame_len = tx_context->frame_len;
-	u16 current_rate = tx_context->tx_rate;
-	u8 need_ack = tx_context->need_ack;
-
-	buf->rts_rrv_time_aa = vnt_get_rtscts_rsvtime_le(priv, 2,
-			tx_context->pkt_type, frame_len, current_rate);
-	buf->rts_rrv_time_ba = vnt_get_rtscts_rsvtime_le(priv, 1,
-			tx_context->pkt_type, frame_len, current_rate);
-	buf->rts_rrv_time_bb = vnt_get_rtscts_rsvtime_le(priv, 0,
-			tx_context->pkt_type, frame_len, current_rate);
-
-	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(priv, tx_context->pkt_type,
-						frame_len, current_rate,
-						need_ack);
-	buf->rrv_time_b = vnt_rxtx_rsvtime_le16(priv, PK_TYPE_11B, frame_len,
-					priv->top_cck_basic_rate, need_ack);
-
-	if (need_mic)
-		head = &tx_head->tx_rts.tx.mic.head;
-
-	return vnt_rxtx_rts_g_head(tx_context, &head->rts_g);
-}
-
-static u16 vnt_rxtx_cts(struct vnt_usb_send_context *tx_context,
-			union vnt_tx_head *tx_head, bool need_mic)
-{
-	struct vnt_private *priv = tx_context->priv;
-	struct vnt_rrv_time_cts *buf = &tx_head->tx_cts.cts;
-	union vnt_tx_data_head *head = &tx_head->tx_cts.tx.head;
-	u32 frame_len = tx_context->frame_len;
-	u16 current_rate = tx_context->tx_rate;
-	u8 need_ack = tx_context->need_ack;
-
-	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(priv, tx_context->pkt_type,
-					frame_len, current_rate, need_ack);
-	buf->rrv_time_b = vnt_rxtx_rsvtime_le16(priv, PK_TYPE_11B,
-				frame_len, priv->top_cck_basic_rate, need_ack);
-
-	buf->cts_rrv_time_ba = vnt_get_rtscts_rsvtime_le(priv, 3,
-			tx_context->pkt_type, frame_len, current_rate);
-
-	if (need_mic)
-		head = &tx_head->tx_cts.tx.mic.head;
-
-	return vnt_fill_cts_head(tx_context, head);
-}
-
-static u16 vnt_rxtx_ab(struct vnt_usb_send_context *tx_context,
-		       union vnt_tx_head *tx_head, bool need_rts, bool need_mic)
-{
-	struct vnt_private *priv = tx_context->priv;
-	struct vnt_rrv_time_ab *buf = &tx_head->tx_ab.ab;
-	union vnt_tx_data_head *head = &tx_head->tx_ab.tx.head;
-	u32 frame_len = tx_context->frame_len;
-	u16 current_rate = tx_context->tx_rate;
-	u8 need_ack = tx_context->need_ack;
-
-	buf->rrv_time = vnt_rxtx_rsvtime_le16(priv, tx_context->pkt_type,
-					      frame_len, current_rate, need_ack);
-
-	if (need_mic)
-		head = &tx_head->tx_ab.tx.mic.head;
-
-	if (need_rts) {
-		if (tx_context->pkt_type == PK_TYPE_11B)
-			buf->rts_rrv_time = vnt_get_rtscts_rsvtime_le(priv, 0,
-				tx_context->pkt_type, frame_len, current_rate);
-		else /* PK_TYPE_11A */
-			buf->rts_rrv_time = vnt_get_rtscts_rsvtime_le(priv, 2,
-				tx_context->pkt_type, frame_len, current_rate);
-
-		return vnt_rxtx_rts_ab_head(tx_context, &head->rts_ab);
-	}
-
-	return vnt_rxtx_datahead_ab(tx_context, &head->data_head_ab);
-}
-
-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)
-{
-	if (tx_context->pkt_type == PK_TYPE_11GB ||
-	    tx_context->pkt_type == PK_TYPE_11GA) {
-		if (need_rts) {
-			if (need_mic)
-				*mic_hdr =
-					&tx_buffer->tx_head.tx_rts.tx.mic.hdr;
-
-			return vnt_rxtx_rts(tx_context, &tx_buffer->tx_head,
-					    need_mic);
-		}
-
-		if (need_mic)
-			*mic_hdr = &tx_buffer->tx_head.tx_cts.tx.mic.hdr;
-
-		return vnt_rxtx_cts(tx_context, &tx_buffer->tx_head, need_mic);
-	}
-
-	if (need_mic)
-		*mic_hdr = &tx_buffer->tx_head.tx_ab.tx.mic.hdr;
-
-	return vnt_rxtx_ab(tx_context, &tx_buffer->tx_head, need_rts, need_mic);
-}
-
-static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
-			   u8 *key_buffer, struct ieee80211_key_conf *tx_key,
-			   struct sk_buff *skb, u16 payload_len,
-			   struct vnt_mic_hdr *mic_hdr)
-{
-	struct ieee80211_hdr *hdr = tx_context->hdr;
+	struct vnt_tx_fifo_head *fifo = &tx_buffer->fifo_head;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_key_conf *tx_key = info->control.hw_key;
+	struct vnt_mic_hdr *mic_hdr;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	u64 pn64;
+	u16 payload_len = skb->len;
 	u8 *iv = ((u8 *)hdr + ieee80211_get_hdrlen_from_skb(skb));
 
 	/* strip header and icv len from payload */
@@ -479,24 +310,31 @@ static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
 	switch (tx_key->cipher) {
 	case WLAN_CIPHER_SUITE_WEP40:
 	case WLAN_CIPHER_SUITE_WEP104:
-		memcpy(key_buffer, iv, 3);
-		memcpy(key_buffer + 3, tx_key->key, tx_key->keylen);
+		memcpy(fifo->tx_key, iv, 3);
+		memcpy(fifo->tx_key + 3, tx_key->key, tx_key->keylen);
 
 		if (tx_key->keylen == WLAN_KEY_LEN_WEP40) {
-			memcpy(key_buffer + 8, iv, 3);
-			memcpy(key_buffer + 11,
+			memcpy(fifo->tx_key + 8, iv, 3);
+			memcpy(fifo->tx_key + 11,
 			       tx_key->key, WLAN_KEY_LEN_WEP40);
 		}
 
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_LEGACY);
 		break;
 	case WLAN_CIPHER_SUITE_TKIP:
-		ieee80211_get_tkip_p2k(tx_key, skb, key_buffer);
+		ieee80211_get_tkip_p2k(tx_key, skb, fifo->tx_key);
 
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_TKIP);
 		break;
 	case WLAN_CIPHER_SUITE_CCMP:
-
-		if (!mic_hdr)
-			return;
+		if (info->control.use_cts_prot) {
+			if (info->control.use_rts)
+				mic_hdr = &tx_buffer->tx_head.tx_rts.tx.mic.hdr;
+			else
+				mic_hdr = &tx_buffer->tx_head.tx_cts.tx.mic.hdr;
+		} else {
+			mic_hdr = &tx_buffer->tx_head.tx_ab.tx.mic.hdr;
+		}
 
 		mic_hdr->id = 0x59;
 		mic_hdr->payload_len = cpu_to_be16(payload_len);
@@ -527,12 +365,137 @@ static void vnt_fill_txkey(struct vnt_usb_send_context *tx_context,
 		if (ieee80211_has_a4(hdr->frame_control))
 			ether_addr_copy(mic_hdr->addr4, hdr->addr4);
 
-		memcpy(key_buffer, tx_key->key, WLAN_KEY_LEN_CCMP);
+		memcpy(fifo->tx_key, tx_key->key, WLAN_KEY_LEN_CCMP);
 
-		break;
+		fifo->frag_ctl |= cpu_to_le16(FRAGCTL_AES);
+		return true;
 	default:
 		break;
 	}
+
+	return false;
+}
+
+static void vnt_rxtx_rts(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_rts *buf = &tx_head->tx_rts.rts;
+	union vnt_tx_data_head *head = &tx_head->tx_rts.tx.head;
+
+	buf->rts_rrv_time_aa = vnt_get_rts_duration(tx_context);
+	buf->rts_rrv_time_ba = buf->rts_rrv_time_aa;
+	buf->rts_rrv_time_bb = buf->rts_rrv_time_aa;
+
+	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(tx_context);
+	buf->rrv_time_b = buf->rrv_time_a;
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_rts.tx.mic.head;
+	}
+
+	vnt_rxtx_rts_g_head(tx_context, &head->rts_g);
+}
+
+static void vnt_rxtx_cts(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_cts *buf = &tx_head->tx_cts.cts;
+	union vnt_tx_data_head *head = &tx_head->tx_cts.tx.head;
+
+	buf->rrv_time_a = vnt_rxtx_rsvtime_le16(tx_context);
+	buf->rrv_time_b = buf->rrv_time_a;
+
+	buf->cts_rrv_time_ba = vnt_get_cts_duration(tx_context);
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_cts.tx.mic.head;
+	}
+
+	vnt_fill_cts_head(tx_context, head);
+}
+
+static void vnt_rxtx_ab(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+	struct vnt_tx_buffer *tx_buffer = tx_context->tx_buffer;
+	union vnt_tx_head *tx_head = &tx_buffer->tx_head;
+	struct vnt_rrv_time_ab *buf = &tx_head->tx_ab.ab;
+	union vnt_tx_data_head *head = &tx_head->tx_ab.tx.head;
+
+	buf->rrv_time = vnt_rxtx_rsvtime_le16(tx_context);
+
+	if (info->control.hw_key) {
+		if (vnt_fill_txkey(tx_buffer, tx_context->skb))
+			head = &tx_head->tx_ab.tx.mic.head;
+	}
+
+	if (info->control.use_rts) {
+		buf->rts_rrv_time = vnt_get_rts_duration(tx_context);
+
+		vnt_rxtx_rts_ab_head(tx_context, &head->rts_ab);
+
+		return;
+	}
+
+	vnt_rxtx_datahead_ab(tx_context, &head->data_head_ab);
+}
+
+static void vnt_generate_tx_parameter(struct vnt_usb_send_context *tx_context)
+{
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_context->skb);
+
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts) {
+			vnt_rxtx_rts(tx_context);
+
+			return;
+		}
+
+		vnt_rxtx_cts(tx_context);
+
+		return;
+	}
+
+	vnt_rxtx_ab(tx_context);
+}
+
+static u16 vnt_get_hdr_size(struct ieee80211_tx_info *info)
+{
+	u16 size = sizeof(struct vnt_tx_datahead_ab);
+
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts)
+			size = sizeof(struct vnt_rts_g);
+		else
+			size = sizeof(struct vnt_cts);
+	} else if (info->control.use_rts) {
+		size = sizeof(struct vnt_rts_ab);
+	}
+
+	if (info->control.hw_key) {
+		if (info->control.hw_key->cipher == WLAN_CIPHER_SUITE_CCMP)
+			size += sizeof(struct vnt_mic_hdr);
+	}
+
+	/* Get rrv_time header */
+	if (info->control.use_cts_prot) {
+		if (info->control.use_rts)
+			size += sizeof(struct vnt_rrv_time_rts);
+		else
+			size += sizeof(struct vnt_rrv_time_cts);
+	} else {
+		size += sizeof(struct vnt_rrv_time_ab);
+	}
+
+	size += sizeof(struct vnt_tx_fifo_head);
+
+	return size;
 }
 
 int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
@@ -540,30 +503,18 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_tx_rate *tx_rate = &info->control.rates[0];
 	struct ieee80211_rate *rate;
-	struct ieee80211_key_conf *tx_key;
 	struct ieee80211_hdr *hdr;
-	struct vnt_mic_hdr *mic_hdr = NULL;
 	struct vnt_tx_buffer *tx_buffer;
 	struct vnt_tx_fifo_head *tx_buffer_head;
 	struct vnt_usb_send_context *tx_context;
 	unsigned long flags;
-	u16 tx_bytes, tx_header_size, tx_body_size, current_rate, duration_id;
 	u8 pkt_type;
-	bool need_rts = false;
-	bool need_mic = false;
 
 	hdr = (struct ieee80211_hdr *)(skb->data);
 
 	rate = ieee80211_get_tx_rate(priv->hw, info);
 
-	current_rate = rate->hw_value;
-	if (priv->current_rate != current_rate &&
-	    !(priv->hw->conf.flags & IEEE80211_CONF_OFFCHANNEL)) {
-		priv->current_rate = current_rate;
-		vnt_schedule_command(priv, WLAN_CMD_SETPOWER);
-	}
-
-	if (current_rate > RATE_11M) {
+	if (rate->hw_value > RATE_11M) {
 		if (info->band == NL80211_BAND_5GHZ) {
 			pkt_type = PK_TYPE_11A;
 		} else {
@@ -589,17 +540,23 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 		return -ENOMEM;
 	}
 
-	tx_context->skb = skb;
 	tx_context->pkt_type = pkt_type;
-	tx_context->need_ack = false;
 	tx_context->frame_len = skb->len + 4;
-	tx_context->tx_rate = current_rate;
+	tx_context->tx_rate =  rate->hw_value;
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	tx_buffer = (struct vnt_tx_buffer *)tx_context->data;
+	tx_context->skb = skb_clone(skb, GFP_ATOMIC);
+	if (!tx_context->skb) {
+		tx_context->in_use = false;
+		return -ENOMEM;
+	}
+
+	tx_buffer = skb_push(skb, vnt_get_hdr_size(info));
+	tx_context->tx_buffer = tx_buffer;
 	tx_buffer_head = &tx_buffer->fifo_head;
-	tx_body_size = skb->len;
+
+	tx_context->type = CONTEXT_DATA_PACKET;
 
 	/*Set fifo controls */
 	if (pkt_type == PK_TYPE_11A)
@@ -623,92 +580,43 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 			cpu_to_le16(DEFAULT_MSDU_LIFETIME_RES_64us);
 	}
 
-	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_NEEDACK);
-		tx_context->need_ack = true;
-	}
 
 	if (ieee80211_has_retry(hdr->frame_control))
 		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LRETRY);
 
-	if (tx_rate->flags & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
-		priv->preamble_type = PREAMBLE_SHORT;
-	else
-		priv->preamble_type = PREAMBLE_LONG;
-
-	if (tx_rate->flags & IEEE80211_TX_RC_USE_RTS_CTS) {
-		need_rts = true;
+	if (info->control.use_rts)
 		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_RTS);
-	}
 
 	if (ieee80211_has_a4(hdr->frame_control))
 		tx_buffer_head->fifo_ctl |= cpu_to_le16(FIFOCTL_LHEAD);
 
 	tx_buffer_head->frag_ctl =
-			cpu_to_le16(ieee80211_get_hdrlen_from_skb(skb) << 10);
+			cpu_to_le16(ieee80211_hdrlen(hdr->frame_control) << 10);
 
-	if (info->control.hw_key) {
-		tx_key = info->control.hw_key;
-		switch (info->control.hw_key->cipher) {
-		case WLAN_CIPHER_SUITE_WEP40:
-		case WLAN_CIPHER_SUITE_WEP104:
-			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_LEGACY);
-			break;
-		case WLAN_CIPHER_SUITE_TKIP:
-			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_TKIP);
-			break;
-		case WLAN_CIPHER_SUITE_CCMP:
-			tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_AES);
-			need_mic = true;
-		default:
-			break;
-		}
-		tx_context->frame_len += tx_key->icv_len;
-	}
+	if (info->control.hw_key)
+		tx_context->frame_len += info->control.hw_key->icv_len;
 
-	tx_buffer_head->current_rate = cpu_to_le16(current_rate);
+	tx_buffer_head->current_rate = cpu_to_le16(rate->hw_value);
 
-	duration_id = vnt_generate_tx_parameter(tx_context, tx_buffer, &mic_hdr,
-						need_mic, need_rts);
-
-	tx_header_size = tx_context->tx_hdr_size;
-	if (!tx_header_size) {
-		tx_context->in_use = false;
-		return -ENOMEM;
-	}
+	vnt_generate_tx_parameter(tx_context);
 
 	tx_buffer_head->frag_ctl |= cpu_to_le16(FRAGCTL_NONFRAG);
 
-	tx_bytes = tx_header_size + tx_body_size;
-
-	memcpy(tx_context->hdr, skb->data, tx_body_size);
-
-	if (info->control.hw_key) {
-		tx_key = info->control.hw_key;
-		if (tx_key->keylen > 0)
-			vnt_fill_txkey(tx_context, tx_buffer_head->tx_key,
-				       tx_key, skb, tx_body_size, mic_hdr);
-	}
-
 	priv->seq_counter = (le16_to_cpu(hdr->seq_ctrl) &
 						IEEE80211_SCTL_SEQ) >> 4;
 
-	tx_buffer->tx_byte_count = cpu_to_le16(tx_bytes);
-	tx_buffer->pkt_no = tx_context->pkt_no;
-	tx_buffer->type = 0x00;
-
-	tx_bytes += 4;
-
-	tx_context->type = CONTEXT_DATA_PACKET;
-	tx_context->buf_len = tx_bytes;
-
 	spin_lock_irqsave(&priv->lock, flags);
 
-	if (vnt_tx_context(priv, tx_context)) {
+	if (vnt_tx_context(priv, tx_context, skb)) {
+		dev_kfree_skb(tx_context->skb);
 		spin_unlock_irqrestore(&priv->lock, flags);
 		return -EIO;
 	}
 
+	dev_kfree_skb(skb);
+
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return 0;
@@ -716,14 +624,13 @@ int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb)
 
 static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
 {
-	struct vnt_beacon_buffer *beacon_buffer;
 	struct vnt_tx_short_buf_head *short_head;
 	struct ieee80211_tx_info *info;
 	struct vnt_usb_send_context *context;
 	struct ieee80211_mgmt *mgmt_hdr;
 	unsigned long flags;
 	u32 frame_size = skb->len + 4;
-	u16 current_rate, count;
+	u16 current_rate;
 
 	spin_lock_irqsave(&priv->lock, flags);
 
@@ -738,8 +645,8 @@ static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	beacon_buffer = (struct vnt_beacon_buffer *)&context->data[0];
-	short_head = &beacon_buffer->short_head;
+	mgmt_hdr = (struct ieee80211_mgmt *)skb->data;
+	short_head = skb_push(skb, sizeof(*short_head));
 
 	if (priv->bb_type == BB_TYPE_11A) {
 		current_rate = RATE_6M;
@@ -764,10 +671,6 @@ static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
 			vnt_time_stamp_off(priv, current_rate);
 	}
 
-	/* Generate Beacon Header */
-	mgmt_hdr = &beacon_buffer->mgmt_hdr;
-	memcpy(mgmt_hdr, skb->data, skb->len);
-
 	/* Get Duration */
 	short_head->duration = mgmt_hdr->duration;
 
@@ -786,18 +689,11 @@ static int vnt_beacon_xmit(struct vnt_private *priv, struct sk_buff *skb)
 	if (priv->seq_counter > 0x0fff)
 		priv->seq_counter = 0;
 
-	count = sizeof(struct vnt_tx_short_buf_head) + skb->len;
-
-	beacon_buffer->tx_byte_count = cpu_to_le16(count);
-	beacon_buffer->pkt_no = context->pkt_no;
-	beacon_buffer->type = 0x01;
-
 	context->type = CONTEXT_BEACON_PACKET;
-	context->buf_len = count + 4; /* USB header */
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-	if (vnt_tx_context(priv, context))
+	if (vnt_tx_context(priv, context, skb))
 		ieee80211_free_txskb(priv->hw, context->skb);
 
 	spin_unlock_irqrestore(&priv->lock, flags);
diff --git a/drivers/staging/vt6656/rxtx.h b/drivers/staging/vt6656/rxtx.h
index 0e6226af7d41..6ca2ca32d036 100644
--- a/drivers/staging/vt6656/rxtx.h
+++ b/drivers/staging/vt6656/rxtx.h
@@ -23,6 +23,13 @@
 #define DEFAULT_MGN_LIFETIME_RES_64us	125  /* 64us */
 #define DEFAULT_MSDU_LIFETIME_RES_64us  8000
 
+/* Length, Service, and Signal fields of Phy for Tx */
+struct vnt_phy_field {
+	u8 signal;
+	u8 service;
+	__le16 len;
+} __packed;
+
 /* MIC HDR data header */
 struct vnt_mic_hdr {
 	u8 id;
@@ -70,14 +77,12 @@ struct vnt_tx_datahead_g {
 	__le16 duration_a;
 	__le16 time_stamp_off_b;
 	__le16 time_stamp_off_a;
-	struct ieee80211_hdr hdr;
 } __packed;
 
 struct vnt_tx_datahead_ab {
 	struct vnt_phy_field ab;
 	__le16 duration;
 	__le16 time_stamp_off;
-	struct ieee80211_hdr hdr;
 } __packed;
 
 /* RTS buffer header */
@@ -155,9 +160,6 @@ struct vnt_tx_fifo_head {
 } __packed;
 
 struct vnt_tx_buffer {
-	u8 type;
-	u8 pkt_no;
-	__le16 tx_byte_count;
 	struct vnt_tx_fifo_head fifo_head;
 	union vnt_tx_head tx_head;
 } __packed;
@@ -170,14 +172,6 @@ struct vnt_tx_short_buf_head {
 	__le16 time_stamp_off;
 } __packed;
 
-struct vnt_beacon_buffer {
-	u8 type;
-	u8 pkt_no;
-	__le16 tx_byte_count;
-	struct vnt_tx_short_buf_head short_head;
-	struct ieee80211_mgmt mgmt_hdr;
-} __packed;
-
 int vnt_tx_packet(struct vnt_private *priv, struct sk_buff *skb);
 int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif);
 int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index 91b62c3dff7b..82b774be6485 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -77,7 +77,7 @@ int vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 data)
 }
 
 int vnt_control_out_blocks(struct vnt_private *priv,
-			   u16 block, u8 reg, u16 length, u8 *data)
+			   u16 block, u8 reg, u16 length, const u8 *data)
 {
 	int ret = 0, i;
 
@@ -196,32 +196,18 @@ static void vnt_int_process_data(struct vnt_private *priv)
 	if (int_data->tsr3 & TSR_VALID)
 		vnt_int_report_rate(priv, int_data->pkt3, int_data->tsr3);
 
-	if (int_data->isr0 != 0) {
-		if (int_data->isr0 & ISR_BNTX &&
-		    priv->op_mode == NL80211_IFTYPE_AP)
-			vnt_schedule_command(priv, WLAN_CMD_BECON_SEND);
-
-		if (int_data->isr0 & ISR_TBTT &&
-		    priv->hw->conf.flags & IEEE80211_CONF_PS) {
-			if (!priv->wake_up_count)
-				priv->wake_up_count =
-					priv->hw->conf.listen_interval;
+	if (!int_data->isr0)
+		return;
 
-			if (priv->wake_up_count)
-				--priv->wake_up_count;
+	if (int_data->isr0 & ISR_BNTX && priv->op_mode == NL80211_IFTYPE_AP)
+		vnt_schedule_command(priv, WLAN_CMD_BECON_SEND);
 
-			/* Turn on wake up to listen next beacon */
-			if (priv->wake_up_count == 1)
-				vnt_schedule_command(priv,
-						     WLAN_CMD_TBTT_WAKEUP);
-		}
-		priv->current_tsf = le64_to_cpu(int_data->tsf);
+	priv->current_tsf = le64_to_cpu(int_data->tsf);
 
-		low_stats->dot11RTSSuccessCount += int_data->rts_success;
-		low_stats->dot11RTSFailureCount += int_data->rts_fail;
-		low_stats->dot11ACKFailureCount += int_data->ack_fail;
-		low_stats->dot11FCSErrorCount += int_data->fcs_err;
-	}
+	low_stats->dot11RTSSuccessCount += int_data->rts_success;
+	low_stats->dot11RTSFailureCount += int_data->rts_fail;
+	low_stats->dot11ACKFailureCount += int_data->ack_fail;
+	low_stats->dot11FCSErrorCount += int_data->fcs_err;
 }
 
 static void vnt_start_interrupt_urb_complete(struct urb *urb)
@@ -442,7 +428,8 @@ static void vnt_tx_context_complete(struct urb *urb)
 
 	switch (urb->status) {
 	case 0:
-		dev_dbg(&priv->usb->dev, "Write %d bytes\n", context->buf_len);
+		dev_dbg(&priv->usb->dev,
+			"Write %d bytes\n", urb->actual_length);
 		break;
 	case -ECONNRESET:
 	case -ENOENT:
@@ -467,30 +454,53 @@ static void vnt_tx_context_complete(struct urb *urb)
 }
 
 int vnt_tx_context(struct vnt_private *priv,
-		   struct vnt_usb_send_context *context)
+		   struct vnt_usb_send_context *context,
+		   struct sk_buff *skb)
 {
+	struct vnt_tx_usb_header *usb;
+	struct urb *urb;
 	int status;
-	struct urb *urb = context->urb;
+	u16 count = skb->len;
+
+	usb = skb_push(skb, sizeof(*usb));
+	usb->tx_byte_count = cpu_to_le16(count);
+	usb->pkt_no = context->pkt_no;
+	usb->type = context->type;
 
 	if (test_bit(DEVICE_FLAGS_DISCONNECTED, &priv->flags)) {
 		context->in_use = false;
 		return -ENODEV;
 	}
 
+	if (skb->len > MAX_TOTAL_SIZE_WITH_ALL_HEADERS) {
+		context->in_use = false;
+		return -E2BIG;
+	}
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		context->in_use = false;
+		return -ENOMEM;
+	}
+
 	usb_fill_bulk_urb(urb,
 			  priv->usb,
 			  usb_sndbulkpipe(priv->usb, 3),
-			  context->data,
-			  context->buf_len,
+			  skb->data,
+			  skb->len,
 			  vnt_tx_context_complete,
 			  context);
 
+	usb_anchor_urb(urb, &priv->tx_submitted);
+
 	status = usb_submit_urb(urb, GFP_ATOMIC);
 	if (status) {
 		dev_dbg(&priv->usb->dev, "Submit Tx URB failed %d\n", status);
-
+		usb_unanchor_urb(urb);
 		context->in_use = false;
 	}
 
+	usb_free_urb(urb);
+
 	return status;
 }
diff --git a/drivers/staging/vt6656/usbpipe.h b/drivers/staging/vt6656/usbpipe.h
index 35697b58d748..52c2a928c9c1 100644
--- a/drivers/staging/vt6656/usbpipe.h
+++ b/drivers/staging/vt6656/usbpipe.h
@@ -41,6 +41,12 @@ struct vnt_interrupt_data {
 	u8 sw[2];
 } __packed;
 
+struct vnt_tx_usb_header {
+	u8 type;
+	u8 pkt_no;
+	__le16 tx_byte_count;
+} __packed;
+
 #define VNT_REG_BLOCK_SIZE	64
 
 int vnt_control_out(struct vnt_private *priv, u8 request, u16 value,
@@ -52,11 +58,12 @@ int vnt_control_out_u8(struct vnt_private *priv, u8 reg, u8 ref_off, u8 data);
 int vnt_control_in_u8(struct vnt_private *priv, u8 reg, u8 reg_off, u8 *data);
 
 int vnt_control_out_blocks(struct vnt_private *priv,
-			   u16 block, u8 reg, u16 len, u8 *data);
+			   u16 block, u8 reg, u16 len, const u8 *data);
 
 int vnt_start_interrupt_urb(struct vnt_private *priv);
 int vnt_submit_rx_urb(struct vnt_private *priv, struct vnt_rcb *rcb);
 int vnt_tx_context(struct vnt_private *priv,
-		   struct vnt_usb_send_context *context);
+		   struct vnt_usb_send_context *context,
+		   struct sk_buff *skb);
 
 #endif /* __USBPIPE_H__ */
diff --git a/drivers/staging/vt6656/wcmd.c b/drivers/staging/vt6656/wcmd.c
index 2c5250ca2801..0ccc87da394e 100644
--- a/drivers/staging/vt6656/wcmd.c
+++ b/drivers/staging/vt6656/wcmd.c
@@ -122,8 +122,7 @@ void vnt_run_command(struct work_struct *work)
 
 	case WLAN_CMD_SETPOWER_START:
 
-		vnt_rf_setpower(priv, priv->current_rate,
-				priv->hw->conf.chandef.chan->hw_value);
+		vnt_rf_setpower(priv, priv->hw->conf.chandef.chan);
 
 		break;
 
diff --git a/drivers/staging/wfx/Makefile b/drivers/staging/wfx/Makefile
index 0d9c1ed092f6..0e0cc982ceab 100644
--- a/drivers/staging/wfx/Makefile
+++ b/drivers/staging/wfx/Makefile
@@ -7,6 +7,7 @@ wfx-y := \
 	bh.o \
 	hwio.o \
 	fwio.o \
+	hif_tx_mib.o \
 	hif_tx.o \
 	hif_rx.o \
 	queue.o \
diff --git a/drivers/staging/wfx/TODO b/drivers/staging/wfx/TODO
index efcb7c6a5aa7..42bf36d43970 100644
--- a/drivers/staging/wfx/TODO
+++ b/drivers/staging/wfx/TODO
@@ -1,60 +1,25 @@
 This is a list of things that need to be done to get this driver out of the
 staging directory.
 
-  - All structures defined in hif_api_*.h are intended to sent/received to/from
-    hardware. All their members whould be declared __le32 or __le16.
-    See:
-       https://lore.kernel.org/lkml/20191111202852.GX26530@ZenIV.linux.org.uk
+  - The HIF API is not yet clean enough.
 
-  - Once previous item done, it will be possible to audit the driver with
-    `sparse'. It will probably find tons of problems with big endian
-    architectures.
-
-  - hif_api_*.h whave been imported from firmware code. Some of the structures
-    are never used in driver.
-
-  - Driver try to maintains power save status of the stations. However, this
-    work is already done by mac80211. sta_asleep_mask and pspoll_mask should be
-    dropped.
-
-  - wfx_tx_queues_get() should be reworked. It currently try compute itself the
-    QoS policy. However, firmware already do the job. Firmware would prefer to
-    have a few packets in each queue and be able to choose itself which queue to
-    use.
+  - The code that check the corectness of received message (in rx_helper()) can
+    be improved. See:
+       https://lore.kernel.org/driverdev-devel/2302785.6C7ODC2LYm@pc-42/
 
   - As suggested by Felix, rate control could be improved following this idea:
         https://lore.kernel.org/lkml/3099559.gv3Q75KnN1@pc-42/
 
-  - When driver is about to loose BSS, it forge its own Null Func request (see
-    wfx_cqm_bssloss_sm()). It should use mechanism provided by mac80211.
-
-  - AP is actually is setup after a call to wfx_bss_info_changed(). Yet,
-    ieee80211_ops provide callback start_ap().
-
-  - The current process for joining a network is incredibly complex. Should be
-    reworked.
-
-  - Monitoring mode is not implemented despite being mandatory by mac80211.
-
-  - "compatible" value are not correct. They should be "vendor,chip". See:
-       https://lore.kernel.org/driverdev-devel/5226570.CMH5hVlZcI@pc-42
-
-  - The "state" field from wfx_vif should be replaced by "vif->type".
-
-  - It seems that wfx_upload_keys() is useless.
-
-  - "event_queue" from wfx_vif seems overkill. These event are rare and they
-     probably could be handled in a simpler fashion.
-
   - Feature called "secure link" should be either developed (using kernel
     crypto API) or dropped.
 
+  - The device allows to filter multicast traffic. The code to support these
+    filters exists in the driver but it is disabled because it has never been
+    tested.
+
   - In wfx_cmd_send(), "async" allow to send command without waiting the reply.
     It may help in some situation, but it is not yet used. In add, it may cause
     some trouble:
       https://lore.kernel.org/driverdev-devel/alpine.DEB.2.21.1910041317381.2992@hadrien/
     So, fix it (by replacing the mutex with a semaphore) or drop it.
 
-  - Chip support P2P, but driver does not implement it.
-
-  - Chip support kind of Mesh, but driver does not implement it.
diff --git a/drivers/staging/wfx/bh.c b/drivers/staging/wfx/bh.c
index 9fcab00a3733..1cbaf8bb4fa3 100644
--- a/drivers/staging/wfx/bh.c
+++ b/drivers/staging/wfx/bh.c
@@ -70,7 +70,7 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 	if (wfx_data_read(wdev, skb->data, alloc_len))
 		goto err;
 
-	piggyback = le16_to_cpup((u16 *)(skb->data + alloc_len - 2));
+	piggyback = le16_to_cpup((__le16 *)(skb->data + alloc_len - 2));
 	_trace_piggyback(piggyback, false);
 
 	hif = (struct hif_msg *)skb->data;
@@ -84,13 +84,12 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 			// piggyback is probably correct.
 			return piggyback;
 		}
-		le16_to_cpus(&hif->len);
-		computed_len = round_up(hif->len - sizeof(hif->len), 16)
-			       + sizeof(struct hif_sl_msg)
-			       + sizeof(struct hif_sl_tag);
+		computed_len =
+			round_up(le16_to_cpu(hif->len) - sizeof(hif->len), 16) +
+			sizeof(struct hif_sl_msg) +
+			sizeof(struct hif_sl_tag);
 	} else {
-		le16_to_cpus(&hif->len);
-		computed_len = round_up(hif->len, 2);
+		computed_len = round_up(le16_to_cpu(hif->len), 2);
 	}
 	if (computed_len != read_len) {
 		dev_err(wdev->dev, "inconsistent message length: %zu != %zu\n",
@@ -103,13 +102,11 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 	if (!(hif->id & HIF_ID_IS_INDICATION)) {
 		(*is_cnf)++;
 		if (hif->id == HIF_CNF_ID_MULTI_TRANSMIT)
-			release_count = le32_to_cpu(((struct hif_cnf_multi_transmit *)hif->body)->num_tx_confs);
+			release_count = ((struct hif_cnf_multi_transmit *)hif->body)->num_tx_confs;
 		else
 			release_count = 1;
 		WARN(wdev->hif.tx_buffers_used < release_count, "corrupted buffer counter");
 		wdev->hif.tx_buffers_used -= release_count;
-		if (!wdev->hif.tx_buffers_used)
-			wake_up(&wdev->hif.tx_buffers_empty);
 	}
 	_trace_hif_recv(hif, wdev->hif.tx_buffers_used);
 
@@ -120,9 +117,11 @@ static int rx_helper(struct wfx_dev *wdev, size_t read_len, int *is_cnf)
 		wdev->hif.rx_seqnum = (hif->seqnum + 1) % (HIF_COUNTER_MAX + 1);
 	}
 
-	skb_put(skb, hif->len);
+	skb_put(skb, le16_to_cpu(hif->len));
 	// wfx_handle_rx takes care on SKB livetime
 	wfx_handle_rx(wdev, skb);
+	if (!wdev->hif.tx_buffers_used)
+		wake_up(&wdev->hif.tx_buffers_empty);
 
 	return piggyback;
 
@@ -307,6 +306,35 @@ void wfx_bh_request_tx(struct wfx_dev *wdev)
 	queue_work(system_highpri_wq, &wdev->hif.bh);
 }
 
+/*
+ * If IRQ is not available, this function allow to manually poll the control
+ * register and simulate an IRQ ahen an event happened.
+ *
+ * Note that the device has a bug: If an IRQ raise while host read control
+ * register, the IRQ is lost. So, use this function carefully (only duing
+ * device initialisation).
+ */
+void wfx_bh_poll_irq(struct wfx_dev *wdev)
+{
+	ktime_t now, start;
+	u32 reg;
+
+	WARN(!wdev->poll_irq, "unexpected IRQ polling can mask IRQ");
+	start = ktime_get();
+	for (;;) {
+		control_reg_read(wdev, &reg);
+		now = ktime_get();
+		if (reg & 0xFFF)
+			break;
+		if (ktime_after(now, ktime_add_ms(start, 1000))) {
+			dev_err(wdev->dev, "time out while polling control register\n");
+			return;
+		}
+		udelay(200);
+	}
+	wfx_bh_request_rx(wdev);
+}
+
 void wfx_bh_register(struct wfx_dev *wdev)
 {
 	INIT_WORK(&wdev->hif.bh, bh_work);
diff --git a/drivers/staging/wfx/bh.h b/drivers/staging/wfx/bh.h
index 93ca98424e0b..4b73437869e1 100644
--- a/drivers/staging/wfx/bh.h
+++ b/drivers/staging/wfx/bh.h
@@ -28,5 +28,6 @@ void wfx_bh_register(struct wfx_dev *wdev);
 void wfx_bh_unregister(struct wfx_dev *wdev);
 void wfx_bh_request_rx(struct wfx_dev *wdev);
 void wfx_bh_request_tx(struct wfx_dev *wdev);
+void wfx_bh_poll_irq(struct wfx_dev *wdev);
 
 #endif /* WFX_BH_H */
diff --git a/drivers/staging/wfx/bus.h b/drivers/staging/wfx/bus.h
index 62d6ecabe4cb..0370b6c59863 100644
--- a/drivers/staging/wfx/bus.h
+++ b/drivers/staging/wfx/bus.h
@@ -25,6 +25,8 @@ struct hwbus_ops {
 			    void *dst, size_t count);
 	int (*copy_to_io)(void *bus_priv, unsigned int addr,
 			  const void *src, size_t count);
+	int (*irq_subscribe)(void *bus_priv);
+	int (*irq_unsubscribe)(void *bus_priv);
 	void (*lock)(void *bus_priv);
 	void (*unlock)(void *bus_priv);
 	size_t (*align_size)(void *bus_priv, size_t size);
diff --git a/drivers/staging/wfx/bus_sdio.c b/drivers/staging/wfx/bus_sdio.c
index dedc3ff58d3e..496bfc8bbacc 100644
--- a/drivers/staging/wfx/bus_sdio.c
+++ b/drivers/staging/wfx/bus_sdio.c
@@ -6,10 +6,12 @@
  * Copyright (c) 2010, ST-Ericsson
  */
 #include <linux/module.h>
+#include <linux/mmc/sdio.h>
 #include <linux/mmc/sdio_func.h>
 #include <linux/mmc/card.h>
 #include <linux/interrupt.h>
 #include <linux/of_irq.h>
+#include <linux/irq.h>
 
 #include "bus.h"
 #include "wfx.h"
@@ -91,53 +93,58 @@ static void wfx_sdio_irq_handler(struct sdio_func *func)
 {
 	struct wfx_sdio_priv *bus = sdio_get_drvdata(func);
 
-	if (bus->core)
-		wfx_bh_request_rx(bus->core);
-	else
-		WARN(!bus->core, "race condition in driver init/deinit");
+	wfx_bh_request_rx(bus->core);
 }
 
 static irqreturn_t wfx_sdio_irq_handler_ext(int irq, void *priv)
 {
 	struct wfx_sdio_priv *bus = priv;
 
-	if (!bus->core) {
-		WARN(!bus->core, "race condition in driver init/deinit");
-		return IRQ_NONE;
-	}
 	sdio_claim_host(bus->func);
 	wfx_bh_request_rx(bus->core);
 	sdio_release_host(bus->func);
 	return IRQ_HANDLED;
 }
 
-static int wfx_sdio_irq_subscribe(struct wfx_sdio_priv *bus)
+static int wfx_sdio_irq_subscribe(void *priv)
 {
+	struct wfx_sdio_priv *bus = priv;
+	u32 flags;
 	int ret;
+	u8 cccr;
 
-	if (bus->of_irq) {
-		ret = request_irq(bus->of_irq, wfx_sdio_irq_handler_ext,
-				  IRQF_TRIGGER_RISING, "wfx", bus);
-	} else {
+	if (!bus->of_irq) {
 		sdio_claim_host(bus->func);
 		ret = sdio_claim_irq(bus->func, wfx_sdio_irq_handler);
 		sdio_release_host(bus->func);
+		return ret;
 	}
-	return ret;
+
+	sdio_claim_host(bus->func);
+	cccr = sdio_f0_readb(bus->func, SDIO_CCCR_IENx, NULL);
+	cccr |= BIT(0);
+	cccr |= BIT(bus->func->num);
+	sdio_f0_writeb(bus->func, cccr, SDIO_CCCR_IENx, NULL);
+	sdio_release_host(bus->func);
+	flags = irq_get_trigger_type(bus->of_irq);
+	if (!flags)
+		flags = IRQF_TRIGGER_HIGH;
+	flags |= IRQF_ONESHOT;
+	return devm_request_threaded_irq(&bus->func->dev, bus->of_irq, NULL,
+					 wfx_sdio_irq_handler_ext, flags,
+					 "wfx", bus);
 }
 
-static int wfx_sdio_irq_unsubscribe(struct wfx_sdio_priv *bus)
+static int wfx_sdio_irq_unsubscribe(void *priv)
 {
+	struct wfx_sdio_priv *bus = priv;
 	int ret;
 
-	if (bus->of_irq) {
-		free_irq(bus->of_irq, bus);
-		ret = 0;
-	} else {
-		sdio_claim_host(bus->func);
-		ret = sdio_release_irq(bus->func);
-		sdio_release_host(bus->func);
-	}
+	if (bus->of_irq)
+		devm_free_irq(&bus->func->dev, bus->of_irq, bus);
+	sdio_claim_host(bus->func);
+	ret = sdio_release_irq(bus->func);
+	sdio_release_host(bus->func);
 	return ret;
 }
 
@@ -151,12 +158,20 @@ static size_t wfx_sdio_align_size(void *priv, size_t size)
 static const struct hwbus_ops wfx_sdio_hwbus_ops = {
 	.copy_from_io = wfx_sdio_copy_from_io,
 	.copy_to_io = wfx_sdio_copy_to_io,
+	.irq_subscribe = wfx_sdio_irq_subscribe,
+	.irq_unsubscribe = wfx_sdio_irq_unsubscribe,
 	.lock			= wfx_sdio_lock,
 	.unlock			= wfx_sdio_unlock,
 	.align_size		= wfx_sdio_align_size,
 };
 
-static const struct of_device_id wfx_sdio_of_match[];
+static const struct of_device_id wfx_sdio_of_match[] = {
+	{ .compatible = "silabs,wfx-sdio" },
+	{ .compatible = "silabs,wf200" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, wfx_sdio_of_match);
+
 static int wfx_sdio_probe(struct sdio_func *func,
 			  const struct sdio_device_id *id)
 {
@@ -165,7 +180,8 @@ static int wfx_sdio_probe(struct sdio_func *func,
 	int ret;
 
 	if (func->num != 1) {
-		dev_err(&func->dev, "SDIO function number is %d while it should always be 1 (unsupported chip?)\n", func->num);
+		dev_err(&func->dev, "SDIO function number is %d while it should always be 1 (unsupported chip?)\n",
+			func->num);
 		return -ENODEV;
 	}
 
@@ -207,18 +223,12 @@ static int wfx_sdio_probe(struct sdio_func *func,
 		goto err1;
 	}
 
-	ret = wfx_sdio_irq_subscribe(bus);
-	if (ret)
-		goto err1;
-
 	ret = wfx_probe(bus->core);
 	if (ret)
-		goto err2;
+		goto err1;
 
 	return 0;
 
-err2:
-	wfx_sdio_irq_unsubscribe(bus);
 err1:
 	sdio_claim_host(func);
 	sdio_disable_func(func);
@@ -232,7 +242,6 @@ static void wfx_sdio_remove(struct sdio_func *func)
 	struct wfx_sdio_priv *bus = sdio_get_drvdata(func);
 
 	wfx_release(bus->core);
-	wfx_sdio_irq_unsubscribe(bus);
 	sdio_claim_host(func);
 	sdio_disable_func(func);
 	sdio_release_host(func);
@@ -248,15 +257,6 @@ static const struct sdio_device_id wfx_sdio_ids[] = {
 };
 MODULE_DEVICE_TABLE(sdio, wfx_sdio_ids);
 
-#ifdef CONFIG_OF
-static const struct of_device_id wfx_sdio_of_match[] = {
-	{ .compatible = "silabs,wfx-sdio" },
-	{ .compatible = "silabs,wf200" },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, wfx_sdio_of_match);
-#endif
-
 struct sdio_driver wfx_sdio_driver = {
 	.name = "wfx-sdio",
 	.id_table = wfx_sdio_ids,
@@ -264,6 +264,6 @@ struct sdio_driver wfx_sdio_driver = {
 	.remove = wfx_sdio_remove,
 	.drv = {
 		.owner = THIS_MODULE,
-		.of_match_table = of_match_ptr(wfx_sdio_of_match),
+		.of_match_table = wfx_sdio_of_match,
 	}
 };
diff --git a/drivers/staging/wfx/bus_spi.c b/drivers/staging/wfx/bus_spi.c
index 61e99b09decb..e8da61fb096b 100644
--- a/drivers/staging/wfx/bus_spi.c
+++ b/drivers/staging/wfx/bus_spi.c
@@ -12,6 +12,7 @@
 #include <linux/gpio/consumer.h>
 #include <linux/spi/spi.h>
 #include <linux/interrupt.h>
+#include <linux/irq.h>
 #include <linux/of.h>
 
 #include "bus.h"
@@ -39,7 +40,6 @@ struct wfx_spi_priv {
 	struct spi_device *func;
 	struct wfx_dev *core;
 	struct gpio_desc *gpio_reset;
-	struct work_struct request_rx;
 	bool need_swab;
 };
 
@@ -140,25 +140,30 @@ static irqreturn_t wfx_spi_irq_handler(int irq, void *priv)
 {
 	struct wfx_spi_priv *bus = priv;
 
-	if (!bus->core) {
-		WARN(!bus->core, "race condition in driver init/deinit");
-		return IRQ_NONE;
-	}
-	queue_work(system_highpri_wq, &bus->request_rx);
+	wfx_bh_request_rx(bus->core);
 	return IRQ_HANDLED;
 }
 
-static void wfx_spi_request_rx(struct work_struct *work)
+static int wfx_spi_irq_subscribe(void *priv)
 {
-	struct wfx_spi_priv *bus =
-		container_of(work, struct wfx_spi_priv, request_rx);
-
-	wfx_bh_request_rx(bus->core);
+	struct wfx_spi_priv *bus = priv;
+	u32 flags;
+
+	flags = irq_get_trigger_type(bus->func->irq);
+	if (!flags)
+		flags = IRQF_TRIGGER_HIGH;
+	flags |= IRQF_ONESHOT;
+	return devm_request_threaded_irq(&bus->func->dev, bus->func->irq, NULL,
+					 wfx_spi_irq_handler, IRQF_ONESHOT,
+					 "wfx", bus);
 }
 
-static void wfx_flush_irq_work(void *w)
+static int wfx_spi_irq_unsubscribe(void *priv)
 {
-	flush_work(w);
+	struct wfx_spi_priv *bus = priv;
+
+	devm_free_irq(&bus->func->dev, bus->func->irq, bus);
+	return 0;
 }
 
 static size_t wfx_spi_align_size(void *priv, size_t size)
@@ -170,6 +175,8 @@ static size_t wfx_spi_align_size(void *priv, size_t size)
 static const struct hwbus_ops wfx_spi_hwbus_ops = {
 	.copy_from_io = wfx_spi_copy_from_io,
 	.copy_to_io = wfx_spi_copy_to_io,
+	.irq_subscribe = wfx_spi_irq_subscribe,
+	.irq_unsubscribe = wfx_spi_irq_unsubscribe,
 	.lock			= wfx_spi_lock,
 	.unlock			= wfx_spi_unlock,
 	.align_size		= wfx_spi_align_size,
@@ -216,22 +223,11 @@ static int wfx_spi_probe(struct spi_device *func)
 		usleep_range(2000, 2500);
 	}
 
-	INIT_WORK(&bus->request_rx, wfx_spi_request_rx);
 	bus->core = wfx_init_common(&func->dev, &wfx_spi_pdata,
 				    &wfx_spi_hwbus_ops, bus);
 	if (!bus->core)
 		return -EIO;
 
-	ret = devm_add_action_or_reset(&func->dev, wfx_flush_irq_work,
-				       &bus->request_rx);
-	if (ret)
-		return ret;
-
-	ret = devm_request_irq(&func->dev, func->irq, wfx_spi_irq_handler,
-			       IRQF_TRIGGER_RISING, "wfx", bus);
-	if (ret)
-		return ret;
-
 	return wfx_probe(bus->core);
 }
 
diff --git a/drivers/staging/wfx/data_rx.c b/drivers/staging/wfx/data_rx.c
index c5b83fedeb55..0e959ebc38b5 100644
--- a/drivers/staging/wfx/data_rx.c
+++ b/drivers/staging/wfx/data_rx.c
@@ -49,7 +49,7 @@ static int wfx_drop_encrypt_data(struct wfx_dev *wdev,
 	}
 
 	/* Firmware strips ICV in case of MIC failure. */
-	if (arg->status == HIF_STATUS_MICFAILURE)
+	if (arg->status == HIF_STATUS_RX_FAIL_MIC)
 		icv_len = 0;
 
 	if (skb->len < hdrlen + iv_len + icv_len) {
@@ -79,7 +79,7 @@ void wfx_rx_cb(struct wfx_vif *wvif,
 	     ieee80211_is_beacon(frame->frame_control)))
 		goto drop;
 
-	if (arg->status == HIF_STATUS_MICFAILURE)
+	if (arg->status == HIF_STATUS_RX_FAIL_MIC)
 		hdr->flag |= RX_FLAG_MMIC_ERROR;
 	else if (arg->status)
 		goto drop;
@@ -118,18 +118,6 @@ void wfx_rx_cb(struct wfx_vif *wvif,
 	    arg->rx_flags.match_uc_addr &&
 	    mgmt->u.action.category == WLAN_CATEGORY_BACK)
 		goto drop;
-	if (ieee80211_is_beacon(frame->frame_control) &&
-	    !arg->status && wvif->vif &&
-	    ether_addr_equal(ieee80211_get_SA(frame),
-			     wvif->vif->bss_conf.bssid)) {
-		/* Disable beacon filter once we're associated... */
-		if (wvif->disable_beacon_filter &&
-		    (wvif->vif->bss_conf.assoc ||
-		     wvif->vif->bss_conf.ibss_joined)) {
-			wvif->disable_beacon_filter = false;
-			schedule_work(&wvif->update_filtering_work);
-		}
-	}
 	ieee80211_rx_irqsafe(wvif->wdev->hw, skb);
 
 	return;
diff --git a/drivers/staging/wfx/data_rx.h b/drivers/staging/wfx/data_rx.h
index 61c28bfd2a37..125dbfc1f875 100644
--- a/drivers/staging/wfx/data_rx.h
+++ b/drivers/staging/wfx/data_rx.h
@@ -8,10 +8,9 @@
 #ifndef WFX_DATA_RX_H
 #define WFX_DATA_RX_H
 
-#include "hif_api_cmd.h"
-
 struct wfx_vif;
 struct sk_buff;
+struct hif_ind_rx;
 
 void wfx_rx_cb(struct wfx_vif *wvif,
 	       const struct hif_ind_rx *arg, struct sk_buff *skb);
diff --git a/drivers/staging/wfx/data_tx.c b/drivers/staging/wfx/data_tx.c
index 42183c70d4df..f042ef36b408 100644
--- a/drivers/staging/wfx/data_tx.c
+++ b/drivers/staging/wfx/data_tx.c
@@ -6,6 +6,7 @@
  * Copyright (c) 2010, ST-Ericsson
  */
 #include <net/mac80211.h>
+#include <linux/etherdevice.h>
 
 #include "data_tx.h"
 #include "wfx.h"
@@ -16,12 +17,11 @@
 #include "traces.h"
 #include "hif_tx_mib.h"
 
-#define WFX_INVALID_RATE_ID    15
-#define WFX_LINK_ID_GC_TIMEOUT ((unsigned long)(10 * HZ))
-
 static int wfx_get_hw_rate(struct wfx_dev *wdev,
 			   const struct ieee80211_tx_rate *rate)
 {
+	struct ieee80211_supported_band *band;
+
 	if (rate->idx < 0)
 		return -1;
 	if (rate->flags & IEEE80211_TX_RC_MCS) {
@@ -33,7 +33,8 @@ static int wfx_get_hw_rate(struct wfx_dev *wdev,
 	}
 	// WFx only support 2GHz, else band information should be retrieved
 	// from ieee80211_tx_info
-	return wdev->hw->wiphy->bands[NL80211_BAND_2GHZ]->bitrates[rate->idx].hw_value;
+	band = wdev->hw->wiphy->bands[NL80211_BAND_2GHZ];
+	return band->bitrates[rate->idx].hw_value;
 }
 
 /* TX policy cache implementation */
@@ -41,73 +42,13 @@ static int wfx_get_hw_rate(struct wfx_dev *wdev,
 static void wfx_tx_policy_build(struct wfx_vif *wvif, struct tx_policy *policy,
 				struct ieee80211_tx_rate *rates)
 {
-	int i;
-	size_t count;
 	struct wfx_dev *wdev = wvif->wdev;
+	int i, rateid;
+	u8 count;
 
 	WARN(rates[0].idx < 0, "invalid rate policy");
 	memset(policy, 0, sizeof(*policy));
-	for (i = 1; i < IEEE80211_TX_MAX_RATES; i++)
-		if (rates[i].idx < 0)
-			break;
-	count = i;
-
-	/* HACK!!! Device has problems (at least) switching from
-	 * 54Mbps CTS to 1Mbps. This switch takes enormous amount
-	 * of time (100-200 ms), leading to valuable throughput drop.
-	 * As a workaround, additional g-rates are injected to the
-	 * policy.
-	 */
-	if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
-	    rates[0].idx > 4 && rates[0].count > 2 &&
-	    rates[1].idx < 2) {
-		int mid_rate = (rates[0].idx + 4) >> 1;
-
-		/* Decrease number of retries for the initial rate */
-		rates[0].count -= 2;
-
-		if (mid_rate != 4) {
-			/* Keep fallback rate at 1Mbps. */
-			rates[3] = rates[1];
-
-			/* Inject 1 transmission on lowest g-rate */
-			rates[2].idx = 4;
-			rates[2].count = 1;
-			rates[2].flags = rates[1].flags;
-
-			/* Inject 1 transmission on mid-rate */
-			rates[1].idx = mid_rate;
-			rates[1].count = 1;
-
-			/* Fallback to 1 Mbps is a really bad thing,
-			 * so let's try to increase probability of
-			 * successful transmission on the lowest g rate
-			 * even more
-			 */
-			if (rates[0].count >= 3) {
-				--rates[0].count;
-				++rates[2].count;
-			}
-
-			/* Adjust amount of rates defined */
-			count += 2;
-		} else {
-			/* Keep fallback rate at 1Mbps. */
-			rates[2] = rates[1];
-
-			/* Inject 2 transmissions on lowest g-rate */
-			rates[1].idx = 4;
-			rates[1].count = 2;
-
-			/* Adjust amount of rates defined */
-			count += 1;
-		}
-	}
-
 	for (i = 0; i < IEEE80211_TX_MAX_RATES; ++i) {
-		int rateid;
-		u8 count;
-
 		if (rates[i].idx < 0)
 			break;
 		WARN_ON(rates[i].count > 15);
@@ -158,8 +99,7 @@ static int wfx_tx_policy_release(struct tx_policy_cache *cache,
 }
 
 static int wfx_tx_policy_get(struct wfx_vif *wvif,
-			     struct ieee80211_tx_rate *rates,
-			     bool *renew)
+			     struct ieee80211_tx_rate *rates, bool *renew)
 {
 	int idx;
 	struct tx_policy_cache *cache = &wvif->tx_policy_cache;
@@ -171,7 +111,7 @@ static int wfx_tx_policy_get(struct wfx_vif *wvif,
 	if (list_empty(&cache->free)) {
 		WARN(1, "unable to get a valid Tx policy");
 		spin_unlock_bh(&cache->lock);
-		return WFX_INVALID_RATE_ID;
+		return HIF_TX_RETRY_POLICY_INVALID;
 	}
 	idx = wfx_tx_policy_find(cache, &wanted);
 	if (idx >= 0) {
@@ -189,10 +129,8 @@ static int wfx_tx_policy_get(struct wfx_vif *wvif,
 		idx = entry - cache->cache;
 	}
 	wfx_tx_policy_use(cache, &cache->cache[idx]);
-	if (list_empty(&cache->free)) {
-		/* Lock TX queues. */
-		wfx_tx_queues_lock(wvif->wdev);
-	}
+	if (list_empty(&cache->free))
+		ieee80211_stop_queues(wvif->wdev->hw);
 	spin_unlock_bh(&cache->lock);
 	return idx;
 }
@@ -202,15 +140,13 @@ static void wfx_tx_policy_put(struct wfx_vif *wvif, int idx)
 	int usage, locked;
 	struct tx_policy_cache *cache = &wvif->tx_policy_cache;
 
-	if (idx == WFX_INVALID_RATE_ID)
+	if (idx == HIF_TX_RETRY_POLICY_INVALID)
 		return;
 	spin_lock_bh(&cache->lock);
 	locked = list_empty(&cache->free);
 	usage = wfx_tx_policy_release(cache, &cache->cache[idx]);
-	if (locked && !usage) {
-		/* Unlock TX queues. */
-		wfx_tx_queues_unlock(wvif->wdev);
-	}
+	if (locked && !usage)
+		ieee80211_wake_queues(wvif->wdev->hw);
 	spin_unlock_bh(&cache->lock);
 }
 
@@ -218,15 +154,17 @@ static int wfx_tx_policy_upload(struct wfx_vif *wvif)
 {
 	struct tx_policy *policies = wvif->tx_policy_cache.cache;
 	u8 tmp_rates[12];
-	int i;
+	int i, is_used;
 
 	do {
 		spin_lock_bh(&wvif->tx_policy_cache.lock);
-		for (i = 0; i < HIF_MIB_NUM_TX_RATE_RETRY_POLICIES; ++i)
-			if (!policies[i].uploaded &&
-			    memzcmp(policies[i].rates, sizeof(policies[i].rates)))
+		for (i = 0; i < ARRAY_SIZE(wvif->tx_policy_cache.cache); ++i) {
+			is_used = memzcmp(policies[i].rates,
+					  sizeof(policies[i].rates));
+			if (!policies[i].uploaded && is_used)
 				break;
-		if (i < HIF_MIB_NUM_TX_RATE_RETRY_POLICIES) {
+		}
+		if (i < ARRAY_SIZE(wvif->tx_policy_cache.cache)) {
 			policies[i].uploaded = true;
 			memcpy(tmp_rates, policies[i].rates, sizeof(tmp_rates));
 			spin_unlock_bh(&wvif->tx_policy_cache.lock);
@@ -234,7 +172,7 @@ static int wfx_tx_policy_upload(struct wfx_vif *wvif)
 		} else {
 			spin_unlock_bh(&wvif->tx_policy_cache.lock);
 		}
-	} while (i < HIF_MIB_NUM_TX_RATE_RETRY_POLICIES);
+	} while (i < ARRAY_SIZE(wvif->tx_policy_cache.cache));
 	return 0;
 }
 
@@ -244,9 +182,7 @@ void wfx_tx_policy_upload_work(struct work_struct *work)
 		container_of(work, struct wfx_vif, tx_policy_upload_work);
 
 	wfx_tx_policy_upload(wvif);
-
 	wfx_tx_unlock(wvif->wdev);
-	wfx_tx_queues_unlock(wvif->wdev);
 }
 
 void wfx_tx_policy_init(struct wfx_vif *wvif)
@@ -260,7 +196,7 @@ void wfx_tx_policy_init(struct wfx_vif *wvif)
 	INIT_LIST_HEAD(&cache->used);
 	INIT_LIST_HEAD(&cache->free);
 
-	for (i = 0; i < HIF_MIB_NUM_TX_RATE_RETRY_POLICIES; ++i)
+	for (i = 0; i < ARRAY_SIZE(cache->cache); ++i)
 		list_add(&cache->cache[i].link, &cache->free);
 }
 
@@ -281,16 +217,11 @@ static void wfx_tx_manage_pm(struct wfx_vif *wvif, struct ieee80211_hdr *hdr,
 			     struct wfx_tx_priv *tx_priv,
 			     struct ieee80211_sta *sta)
 {
-	u32 mask = ~BIT(tx_priv->raw_link_id);
 	struct wfx_sta_priv *sta_priv;
 	int tid = ieee80211_get_tid(hdr);
 
-	spin_lock_bh(&wvif->ps_state_lock);
-	if (ieee80211_is_auth(hdr->frame_control))
-		wvif->sta_asleep_mask &= mask;
-	spin_unlock_bh(&wvif->ps_state_lock);
-
 	if (sta) {
+		tx_priv->has_sta = true;
 		sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
 		spin_lock_bh(&sta_priv->lock);
 		sta_priv->buffered[tid]++;
@@ -299,9 +230,8 @@ static void wfx_tx_manage_pm(struct wfx_vif *wvif, struct ieee80211_hdr *hdr,
 	}
 }
 
-static u8 wfx_tx_get_raw_link_id(struct wfx_vif *wvif,
-				 struct ieee80211_sta *sta,
-				 struct ieee80211_hdr *hdr)
+static u8 wfx_tx_get_link_id(struct wfx_vif *wvif, struct ieee80211_sta *sta,
+			     struct ieee80211_hdr *hdr)
 {
 	struct wfx_sta_priv *sta_priv =
 		sta ? (struct wfx_sta_priv *)&sta->drv_priv : NULL;
@@ -313,7 +243,7 @@ static u8 wfx_tx_get_raw_link_id(struct wfx_vif *wvif,
 		return 0;
 	if (is_multicast_ether_addr(da))
 		return 0;
-	return WFX_LINK_ID_NO_ASSOC;
+	return HIF_LINK_ID_NOT_ASSOCIATED;
 }
 
 static void wfx_tx_fixup_rates(struct ieee80211_tx_rate *rates)
@@ -358,7 +288,8 @@ static void wfx_tx_fixup_rates(struct ieee80211_tx_rate *rates)
 		if (rates[i].idx == -1) {
 			rates[i].idx = 0;
 			rates[i].count = 8; // == hw->max_rate_tries
-			rates[i].flags = rates[i - 1].flags & IEEE80211_TX_RC_MCS;
+			rates[i].flags = rates[i - 1].flags &
+					 IEEE80211_TX_RC_MCS;
 			break;
 		}
 	}
@@ -375,24 +306,19 @@ static u8 wfx_tx_get_rate_id(struct wfx_vif *wvif,
 
 	rate_id = wfx_tx_policy_get(wvif,
 				    tx_info->driver_rates, &tx_policy_renew);
-	if (rate_id == WFX_INVALID_RATE_ID)
+	if (rate_id == HIF_TX_RETRY_POLICY_INVALID)
 		dev_warn(wvif->wdev->dev, "unable to get a valid Tx policy");
 
 	if (tx_policy_renew) {
-		/* FIXME: It's not so optimal to stop TX queues every now and
-		 * then.  Better to reimplement task scheduling with a counter.
-		 */
 		wfx_tx_lock(wvif->wdev);
-		wfx_tx_queues_lock(wvif->wdev);
-		if (!schedule_work(&wvif->tx_policy_upload_work)) {
-			wfx_tx_queues_unlock(wvif->wdev);
+		if (!schedule_work(&wvif->tx_policy_upload_work))
 			wfx_tx_unlock(wvif->wdev);
-		}
 	}
 	return rate_id;
 }
 
-static struct hif_ht_tx_parameters wfx_tx_get_tx_parms(struct wfx_dev *wdev, struct ieee80211_tx_info *tx_info)
+static struct hif_ht_tx_parameters wfx_tx_get_tx_parms(struct wfx_dev *wdev,
+						       struct ieee80211_tx_info *tx_info)
 {
 	struct ieee80211_tx_rate *rate = &tx_info->driver_rates[0];
 	struct hif_ht_tx_parameters ret = { };
@@ -429,7 +355,7 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_key_conf *hw_key = tx_info->control.hw_key;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	int queue_id = tx_info->hw_queue;
+	int queue_id = skb_get_queue_mapping(skb);
 	size_t offset = (size_t)skb->data & 3;
 	int wmsg_len = sizeof(struct hif_msg) +
 			sizeof(struct hif_req_tx) + offset;
@@ -441,14 +367,8 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	memset(tx_info->rate_driver_data, 0, sizeof(struct wfx_tx_priv));
 	// Fill tx_priv
 	tx_priv = (struct wfx_tx_priv *)tx_info->rate_driver_data;
-	tx_priv->raw_link_id = wfx_tx_get_raw_link_id(wvif, sta, hdr);
-	tx_priv->link_id = tx_priv->raw_link_id;
 	if (ieee80211_has_protected(hdr->frame_control))
 		tx_priv->hw_key = hw_key;
-	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
-		tx_priv->link_id = WFX_LINK_ID_AFTER_DTIM;
-	if (sta && (sta->uapsd_queues & BIT(queue_id)))
-		tx_priv->link_id = WFX_LINK_ID_UAPSD;
 
 	// Fill hif_msg
 	WARN(skb_headroom(skb) < wmsg_len, "not enough space in skb");
@@ -461,7 +381,8 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	hif_msg->id = HIF_REQ_ID_TX;
 	hif_msg->interface = wvif->id;
 	if (skb->len > wvif->wdev->hw_caps.size_inp_ch_buf) {
-		dev_warn(wvif->wdev->dev, "requested frame size (%d) is larger than maximum supported (%d)\n",
+		dev_warn(wvif->wdev->dev,
+			 "requested frame size (%d) is larger than maximum supported (%d)\n",
 			 skb->len, wvif->wdev->hw_caps.size_inp_ch_buf);
 		skb_pull(skb, wmsg_len);
 		return -EIO;
@@ -472,13 +393,14 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	// packet_id just need to be unique on device. 32bits are more than
 	// necessary for that task, so we tae advantage of it to add some extra
 	// data for debug.
-	req->packet_id = queue_id << 28 |
-			 IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl)) << 16 |
-			 (atomic_add_return(1, &wvif->wdev->packet_id) & 0xFFFF);
+	req->packet_id = atomic_add_return(1, &wvif->wdev->packet_id) & 0xFFFF;
+	req->packet_id |= IEEE80211_SEQ_TO_SN(le16_to_cpu(hdr->seq_ctrl)) << 16;
+	req->packet_id |= queue_id << 28;
+
 	req->data_flags.fc_offset = offset;
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
 		req->data_flags.after_dtim = 1;
-	req->queue_id.peer_sta_id = tx_priv->raw_link_id;
+	req->queue_id.peer_sta_id = wfx_tx_get_link_id(wvif, sta, hdr);
 	// Queue index are inverted between firmware and Linux
 	req->queue_id.queue_id = 3 - queue_id;
 	req->ht_tx_parameters = wfx_tx_get_tx_parms(wvif->wdev, tx_info);
@@ -486,7 +408,7 @@ static int wfx_tx_inner(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 
 	// Auxiliary operations
 	wfx_tx_manage_pm(wvif, hdr, tx_priv, sta);
-	wfx_tx_queue_put(wvif->wdev, &wvif->wdev->tx_queue[queue_id], skb);
+	wfx_tx_queues_put(wvif->wdev, skb);
 	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
 		schedule_work(&wvif->update_tim_work);
 	wfx_bh_request_tx(wvif->wdev);
@@ -528,28 +450,59 @@ drop:
 	ieee80211_tx_status_irqsafe(wdev->hw, skb);
 }
 
-void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
+static struct ieee80211_hdr *wfx_skb_hdr80211(struct sk_buff *skb)
 {
-	int i;
-	int tx_count;
-	struct sk_buff *skb;
-	struct ieee80211_tx_rate *rate;
-	struct ieee80211_tx_info *tx_info;
-	const struct wfx_tx_priv *tx_priv;
+	struct hif_msg *hif = (struct hif_msg *)skb->data;
+	struct hif_req_tx *req = (struct hif_req_tx *)hif->body;
 
-	skb = wfx_pending_get(wvif->wdev, arg->packet_id);
-	if (!skb) {
-		dev_warn(wvif->wdev->dev,
-			 "received unknown packet_id (%#.8x) from chip\n",
-			 arg->packet_id);
-		return;
+	return (struct ieee80211_hdr *)(req->frame + req->data_flags.fc_offset);
+}
+
+static void wfx_tx_update_sta(struct wfx_vif *wvif, struct ieee80211_hdr *hdr)
+{
+	int tid = ieee80211_get_tid(hdr);
+	struct wfx_sta_priv *sta_priv;
+	struct ieee80211_sta *sta;
+
+	rcu_read_lock(); // protect sta
+	sta = ieee80211_find_sta(wvif->vif, hdr->addr1);
+	if (sta) {
+		sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
+		spin_lock_bh(&sta_priv->lock);
+		WARN(!sta_priv->buffered[tid], "inconsistent notification");
+		sta_priv->buffered[tid]--;
+		if (!sta_priv->buffered[tid])
+			ieee80211_sta_set_buffered(sta, tid, false);
+		spin_unlock_bh(&sta_priv->lock);
+	} else {
+		dev_dbg(wvif->wdev->dev, "%s: sta does not exist anymore\n",
+			__func__);
 	}
-	tx_info = IEEE80211_SKB_CB(skb);
-	tx_priv = wfx_skb_tx_priv(skb);
-	_trace_tx_stats(arg, skb,
-			wfx_pending_get_pkt_us_delay(wvif->wdev, skb));
+	rcu_read_unlock();
+}
+
+static void wfx_skb_dtor(struct wfx_vif *wvif, struct sk_buff *skb)
+{
+	struct hif_msg *hif = (struct hif_msg *)skb->data;
+	struct hif_req_tx *req = (struct hif_req_tx *)hif->body;
+	unsigned int offset = sizeof(struct hif_msg) +
+			      sizeof(struct hif_req_tx) +
+			      req->data_flags.fc_offset;
+
+	WARN_ON(!wvif);
+	wfx_tx_policy_put(wvif, req->tx_flags.retry_policy_index);
+	skb_pull(skb, offset);
+	ieee80211_tx_status_irqsafe(wvif->wdev->hw, skb);
+}
+
+static void wfx_tx_fill_rates(struct wfx_dev *wdev,
+			      struct ieee80211_tx_info *tx_info,
+			      const struct hif_cnf_tx *arg)
+{
+	struct ieee80211_tx_rate *rate;
+	int tx_count;
+	int i;
 
-	// You can touch to tx_priv, but don't touch to tx_info->status.
 	tx_count = arg->ack_failures;
 	if (!arg->status || arg->ack_failures)
 		tx_count += 1; // Also report success
@@ -558,16 +511,14 @@ void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
 		if (rate->idx < 0)
 			break;
 		if (tx_count < rate->count &&
-		    arg->status == HIF_STATUS_RETRY_EXCEEDED &&
+		    arg->status == HIF_STATUS_TX_FAIL_RETRIES &&
 		    arg->ack_failures)
-			dev_dbg(wvif->wdev->dev, "all retries were not consumed: %d != %d\n",
+			dev_dbg(wdev->dev, "all retries were not consumed: %d != %d\n",
 				rate->count, tx_count);
 		if (tx_count <= rate->count && tx_count &&
-		    arg->txed_rate != wfx_get_hw_rate(wvif->wdev, rate))
-			dev_dbg(wvif->wdev->dev,
-				"inconsistent tx_info rates: %d != %d\n",
-				arg->txed_rate,
-				wfx_get_hw_rate(wvif->wdev, rate));
+		    arg->txed_rate != wfx_get_hw_rate(wdev, rate))
+			dev_dbg(wdev->dev, "inconsistent tx_info rates: %d != %d\n",
+				arg->txed_rate, wfx_get_hw_rate(wdev, rate));
 		if (tx_count > rate->count) {
 			tx_count -= rate->count;
 		} else if (!tx_count) {
@@ -579,8 +530,30 @@ void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
 		}
 	}
 	if (tx_count)
-		dev_dbg(wvif->wdev->dev,
-			"%d more retries than expected\n", tx_count);
+		dev_dbg(wdev->dev, "%d more retries than expected\n", tx_count);
+}
+
+void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
+{
+	struct ieee80211_tx_info *tx_info;
+	const struct wfx_tx_priv *tx_priv;
+	struct sk_buff *skb;
+
+	skb = wfx_pending_get(wvif->wdev, arg->packet_id);
+	if (!skb) {
+		dev_warn(wvif->wdev->dev, "received unknown packet_id (%#.8x) from chip\n",
+			 arg->packet_id);
+		return;
+	}
+	tx_info = IEEE80211_SKB_CB(skb);
+	tx_priv = wfx_skb_tx_priv(skb);
+	_trace_tx_stats(arg, skb,
+			wfx_pending_get_pkt_us_delay(wvif->wdev, skb));
+
+	// You can touch to tx_priv, but don't touch to tx_info->status.
+	wfx_tx_fill_rates(wvif->wdev, tx_info, arg);
+	if (tx_priv->has_sta)
+		wfx_tx_update_sta(wvif, wfx_skb_hdr80211(skb));
 	skb_trim(skb, skb->len - wfx_tx_get_icv_len(tx_priv->hw_key));
 
 	// From now, you can touch to tx_info->status, but do not touch to
@@ -590,63 +563,64 @@ void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg)
 	memset(tx_info->pad, 0, sizeof(tx_info->pad));
 
 	if (!arg->status) {
-		if (wvif->bss_loss_state &&
-		    arg->packet_id == wvif->bss_loss_confirm_id)
-			wfx_cqm_bssloss_sm(wvif, 0, 1, 0);
 		tx_info->status.tx_time =
-		arg->media_delay - arg->tx_queue_delay;
+			le32_to_cpu(arg->media_delay) -
+			le32_to_cpu(arg->tx_queue_delay);
 		if (tx_info->flags & IEEE80211_TX_CTL_NO_ACK)
 			tx_info->flags |= IEEE80211_TX_STAT_NOACK_TRANSMITTED;
 		else
 			tx_info->flags |= IEEE80211_TX_STAT_ACK;
-	} else if (arg->status == HIF_REQUEUE) {
-		WARN(!arg->tx_result_flags.requeue, "incoherent status and result_flags");
+	} else if (arg->status == HIF_STATUS_TX_FAIL_REQUEUE) {
+		WARN(!arg->tx_result_flags.requeue,
+		     "incoherent status and result_flags");
 		if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 			wvif->after_dtim_tx_allowed = false; // DTIM period elapsed
 			schedule_work(&wvif->update_tim_work);
 		}
 		tx_info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
-	} else {
-		if (wvif->bss_loss_state &&
-		    arg->packet_id == wvif->bss_loss_confirm_id)
-			wfx_cqm_bssloss_sm(wvif, 0, 0, 1);
 	}
-	wfx_pending_remove(wvif->wdev, skb);
+	wfx_skb_dtor(wvif, skb);
 }
 
-static void wfx_notify_buffered_tx(struct wfx_vif *wvif, struct sk_buff *skb)
+void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	       u32 queues, bool drop)
 {
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	struct ieee80211_sta *sta;
-	struct wfx_sta_priv *sta_priv;
-	int tid = ieee80211_get_tid(hdr);
+	struct wfx_dev *wdev = hw->priv;
+	struct sk_buff_head dropped;
+	struct wfx_queue *queue;
+	struct wfx_vif *wvif;
+	struct hif_msg *hif;
+	struct sk_buff *skb;
+	int vif_id = -1;
+	int i;
 
-	rcu_read_lock(); // protect sta
-	sta = ieee80211_find_sta(wvif->vif, hdr->addr1);
-	if (sta) {
-		sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
-		spin_lock_bh(&sta_priv->lock);
-		WARN(!sta_priv->buffered[tid], "inconsistent notification");
-		sta_priv->buffered[tid]--;
-		if (!sta_priv->buffered[tid])
-			ieee80211_sta_set_buffered(sta, tid, false);
-		spin_unlock_bh(&sta_priv->lock);
+	if (vif)
+		vif_id = ((struct wfx_vif *)vif->drv_priv)->id;
+	skb_queue_head_init(&dropped);
+	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+		if (!(BIT(i) & queues))
+			continue;
+		queue = &wdev->tx_queue[i];
+		if (drop)
+			wfx_tx_queue_drop(wdev, queue, vif_id, &dropped);
+		if (wdev->chip_frozen)
+			continue;
+		if (wait_event_timeout(wdev->tx_dequeue,
+				       wfx_tx_queue_empty(wdev, queue, vif_id),
+				       msecs_to_jiffies(1000)) <= 0)
+			dev_warn(wdev->dev,
+				 "frames queued while flushing tx queues?");
+	}
+	wfx_tx_flush(wdev);
+	if (wdev->chip_frozen)
+		wfx_pending_drop(wdev, &dropped);
+	while ((skb = skb_dequeue(&dropped)) != NULL) {
+		hif = (struct hif_msg *)skb->data;
+		wvif = wdev_to_wvif(wdev, hif->interface);
+		if (wfx_skb_tx_priv(skb)->has_sta)
+			wfx_tx_update_sta(wvif, wfx_skb_hdr80211(skb));
+		ieee80211_tx_info_clear_status(IEEE80211_SKB_CB(skb));
+		wfx_skb_dtor(wvif, skb);
 	}
-	rcu_read_unlock();
 }
 
-void wfx_skb_dtor(struct wfx_dev *wdev, struct sk_buff *skb)
-{
-	struct hif_msg *hif = (struct hif_msg *)skb->data;
-	struct hif_req_tx *req = (struct hif_req_tx *)hif->body;
-	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
-	unsigned int offset = sizeof(struct hif_req_tx) +
-				sizeof(struct hif_msg) +
-				req->data_flags.fc_offset;
-
-	WARN_ON(!wvif);
-	skb_pull(skb, offset);
-	wfx_notify_buffered_tx(wvif, skb);
-	wfx_tx_policy_put(wvif, req->tx_flags.retry_policy_index);
-	ieee80211_tx_status_irqsafe(wdev->hw, skb);
-}
diff --git a/drivers/staging/wfx/data_tx.h b/drivers/staging/wfx/data_tx.h
index c545dd75449b..54fff24508fb 100644
--- a/drivers/staging/wfx/data_tx.h
+++ b/drivers/staging/wfx/data_tx.h
@@ -26,7 +26,7 @@ struct tx_policy {
 };
 
 struct tx_policy_cache {
-	struct tx_policy cache[HIF_MIB_NUM_TX_RATE_RETRY_POLICIES];
+	struct tx_policy cache[HIF_TX_RETRY_POLICY_MAX];
 	// FIXME: use a trees and drop hash from tx_policy
 	struct list_head used;
 	struct list_head free;
@@ -36,8 +36,7 @@ struct tx_policy_cache {
 struct wfx_tx_priv {
 	ktime_t xmit_timestamp;
 	struct ieee80211_key_conf *hw_key;
-	u8 link_id;
-	u8 raw_link_id;
+	bool has_sta;
 } __packed;
 
 void wfx_tx_policy_init(struct wfx_vif *wvif);
@@ -46,7 +45,8 @@ void wfx_tx_policy_upload_work(struct work_struct *work);
 void wfx_tx(struct ieee80211_hw *hw, struct ieee80211_tx_control *control,
 	    struct sk_buff *skb);
 void wfx_tx_confirm_cb(struct wfx_vif *wvif, const struct hif_cnf_tx *arg);
-void wfx_skb_dtor(struct wfx_dev *wdev, struct sk_buff *skb);
+void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	       u32 queues, bool drop);
 
 static inline struct wfx_tx_priv *wfx_skb_tx_priv(struct sk_buff *skb)
 {
diff --git a/drivers/staging/wfx/debug.c b/drivers/staging/wfx/debug.c
index 1164aba118a1..10d649985696 100644
--- a/drivers/staging/wfx/debug.c
+++ b/drivers/staging/wfx/debug.c
@@ -61,19 +61,26 @@ const char *get_reg_name(unsigned long id)
 
 static int wfx_counters_show(struct seq_file *seq, void *v)
 {
-	int ret;
+	int ret, i;
 	struct wfx_dev *wdev = seq->private;
-	struct hif_mib_extended_count_table counters;
+	struct hif_mib_extended_count_table counters[3];
+
+	for (i = 0; i < ARRAY_SIZE(counters); i++) {
+		ret = hif_get_counters_table(wdev, i, counters + i);
+		if (ret < 0)
+			return ret;
+		if (ret > 0)
+			return -EIO;
+	}
 
-	ret = hif_get_counters_table(wdev, &counters);
-	if (ret < 0)
-		return ret;
-	if (ret > 0)
-		return -EIO;
+	seq_printf(seq, "%-24s %12s %12s %12s\n",
+		   "", "global", "iface 0", "iface 1");
 
 #define PUT_COUNTER(name) \
-	seq_printf(seq, "%24s %d\n", #name ":",\
-		   le32_to_cpu(counters.count_##name))
+	seq_printf(seq, "%-24s %12d %12d %12d\n", #name, \
+		   le32_to_cpu(counters[2].count_##name), \
+		   le32_to_cpu(counters[0].count_##name), \
+		   le32_to_cpu(counters[1].count_##name))
 
 	PUT_COUNTER(tx_packets);
 	PUT_COUNTER(tx_multicast_frames);
@@ -105,6 +112,12 @@ static int wfx_counters_show(struct seq_file *seq, void *v)
 
 #undef PUT_COUNTER
 
+	for (i = 0; i < ARRAY_SIZE(counters[0].reserved); i++)
+		seq_printf(seq, "reserved[%02d]%12s %12d %12d %12d\n", i, "",
+			   le32_to_cpu(counters[2].reserved[i]),
+			   le32_to_cpu(counters[0].reserved[i]),
+			   le32_to_cpu(counters[1].reserved[i]));
+
 	return 0;
 }
 DEFINE_SHOW_ATTRIBUTE(wfx_counters);
@@ -142,7 +155,7 @@ static int wfx_rx_stats_show(struct seq_file *seq, void *v)
 	mutex_lock(&wdev->rx_stats_lock);
 	seq_printf(seq, "Timestamp: %dus\n", st->date);
 	seq_printf(seq, "Low power clock: frequency %uHz, external %s\n",
-		   st->pwr_clk_freq,
+		   le32_to_cpu(st->pwr_clk_freq),
 		   st->is_ext_pwr_clk ? "yes" : "no");
 	seq_printf(seq,
 		   "Num. of frames: %d, PER (x10e4): %d, Throughput: %dKbps/s\n",
@@ -152,9 +165,12 @@ static int wfx_rx_stats_show(struct seq_file *seq, void *v)
 	for (i = 0; i < ARRAY_SIZE(channel_names); i++) {
 		if (channel_names[i])
 			seq_printf(seq, "%5s %8d %8d %8d %8d %8d\n",
-				   channel_names[i], st->nb_rx_by_rate[i],
-				   st->per[i], st->rssi[i] / 100,
-				   st->snr[i] / 100, st->cfo[i]);
+				   channel_names[i],
+				   le32_to_cpu(st->nb_rx_by_rate[i]),
+				   le16_to_cpu(st->per[i]),
+				   (s16)le16_to_cpu(st->rssi[i]) / 100,
+				   (s16)le16_to_cpu(st->snr[i]) / 100,
+				   (s16)le16_to_cpu(st->cfo[i]));
 	}
 	mutex_unlock(&wdev->rx_stats_lock);
 
@@ -162,6 +178,30 @@ static int wfx_rx_stats_show(struct seq_file *seq, void *v)
 }
 DEFINE_SHOW_ATTRIBUTE(wfx_rx_stats);
 
+static int wfx_tx_power_loop_show(struct seq_file *seq, void *v)
+{
+	struct wfx_dev *wdev = seq->private;
+	struct hif_tx_power_loop_info *st = &wdev->tx_power_loop_info;
+	int tmp;
+
+	mutex_lock(&wdev->tx_power_loop_info_lock);
+	tmp = le16_to_cpu(st->tx_gain_dig);
+	seq_printf(seq, "Tx gain digital: %d\n", tmp);
+	tmp = le16_to_cpu(st->tx_gain_pa);
+	seq_printf(seq, "Tx gain PA: %d\n", tmp);
+	tmp = (s16)le16_to_cpu(st->target_pout);
+	seq_printf(seq, "Target Pout: %d.%02d dBm\n", tmp / 4, (tmp % 4) * 25);
+	tmp = (s16)le16_to_cpu(st->p_estimation);
+	seq_printf(seq, "FEM Pout: %d.%02d dBm\n", tmp / 4, (tmp % 4) * 25);
+	tmp = le16_to_cpu(st->vpdet);
+	seq_printf(seq, "Vpdet: %d mV\n", tmp);
+	seq_printf(seq, "Measure index: %d\n", st->measurement_index);
+	mutex_unlock(&wdev->tx_power_loop_info_lock);
+
+	return 0;
+}
+DEFINE_SHOW_ATTRIBUTE(wfx_tx_power_loop);
+
 static ssize_t wfx_send_pds_write(struct file *file,
 				  const char __user *user_buf,
 				  size_t count, loff_t *ppos)
@@ -234,7 +274,7 @@ static ssize_t wfx_send_hif_msg_write(struct file *file,
 	request = memdup_user(user_buf, count);
 	if (IS_ERR(request))
 		return PTR_ERR(request);
-	if (request->len != count) {
+	if (le16_to_cpu(request->len) != count) {
 		kfree(request);
 		return -EINVAL;
 	}
@@ -301,6 +341,8 @@ int wfx_debug_init(struct wfx_dev *wdev)
 	d = debugfs_create_dir("wfx", wdev->hw->wiphy->debugfsdir);
 	debugfs_create_file("counters", 0444, d, wdev, &wfx_counters_fops);
 	debugfs_create_file("rx_stats", 0444, d, wdev, &wfx_rx_stats_fops);
+	debugfs_create_file("tx_power_loop", 0444, d, wdev,
+			    &wfx_tx_power_loop_fops);
 	debugfs_create_file("send_pds", 0200, d, wdev, &wfx_send_pds_fops);
 	debugfs_create_file("burn_slk_key", 0200, d, wdev,
 			    &wfx_burn_slk_key_fops);
diff --git a/drivers/staging/wfx/fwio.c b/drivers/staging/wfx/fwio.c
index 9d61082c1e6c..72bb3d2a9613 100644
--- a/drivers/staging/wfx/fwio.c
+++ b/drivers/staging/wfx/fwio.c
@@ -99,16 +99,16 @@ static int sram_write_dma_safe(struct wfx_dev *wdev, u32 addr, const u8 *buf,
 	return ret;
 }
 
-int get_firmware(struct wfx_dev *wdev, u32 keyset_chip,
-		 const struct firmware **fw, int *file_offset)
+static int get_firmware(struct wfx_dev *wdev, u32 keyset_chip,
+			const struct firmware **fw, int *file_offset)
 {
 	int keyset_file;
 	char filename[256];
 	const char *data;
 	int ret;
 
-	snprintf(filename, sizeof(filename), "%s_%02X.sec", wdev->pdata.file_fw,
-		 keyset_chip);
+	snprintf(filename, sizeof(filename), "%s_%02X.sec",
+		 wdev->pdata.file_fw, keyset_chip);
 	ret = firmware_request_nowarn(fw, filename, wdev->dev);
 	if (ret) {
 		dev_info(wdev->dev, "can't load %s, falling back to %s.sec\n",
@@ -325,8 +325,8 @@ static int init_gpr(struct wfx_dev *wdev)
 				     gpr_init[i].value);
 		if (ret < 0)
 			return ret;
-		dev_dbg(wdev->dev, "  index %02x: %08x\n", gpr_init[i].index,
-			gpr_init[i].value);
+		dev_dbg(wdev->dev, "  index %02x: %08x\n",
+			gpr_init[i].index, gpr_init[i].value);
 	}
 	return 0;
 }
@@ -360,7 +360,7 @@ int wfx_init_device(struct wfx_dev *wdev)
 	dev_dbg(wdev->dev, "initial config register value: %08x\n", reg);
 
 	hw_revision = FIELD_GET(CFG_DEVICE_ID_MAJOR, reg);
-	if (hw_revision == 0 || hw_revision > 2) {
+	if (hw_revision == 0) {
 		dev_err(wdev->dev, "bad hardware revision number: %d\n",
 			hw_revision);
 		return -ENODEV;
diff --git a/drivers/staging/wfx/hif_api_cmd.h b/drivers/staging/wfx/hif_api_cmd.h
index 071b71e2a107..21cde19cff75 100644
--- a/drivers/staging/wfx/hif_api_cmd.h
+++ b/drivers/staging/wfx/hif_api_cmd.h
@@ -10,56 +10,54 @@
 
 #include "hif_api_general.h"
 
-#define HIF_NUM_AC                             4
-
 #define HIF_API_SSID_SIZE                      API_SSID_SIZE
 
 enum hif_requests_ids {
-	HIF_REQ_ID_RESET                             = 0x0a,
-	HIF_REQ_ID_READ_MIB                          = 0x05,
-	HIF_REQ_ID_WRITE_MIB                         = 0x06,
-	HIF_REQ_ID_START_SCAN                        = 0x07,
-	HIF_REQ_ID_STOP_SCAN                         = 0x08,
-	HIF_REQ_ID_TX                                = 0x04,
-	HIF_REQ_ID_JOIN                              = 0x0b,
-	HIF_REQ_ID_SET_PM_MODE                       = 0x10,
-	HIF_REQ_ID_SET_BSS_PARAMS                    = 0x11,
-	HIF_REQ_ID_ADD_KEY                           = 0x0c,
-	HIF_REQ_ID_REMOVE_KEY                        = 0x0d,
-	HIF_REQ_ID_EDCA_QUEUE_PARAMS                 = 0x13,
-	HIF_REQ_ID_START                             = 0x17,
-	HIF_REQ_ID_BEACON_TRANSMIT                   = 0x18,
-	HIF_REQ_ID_UPDATE_IE                         = 0x1b,
-	HIF_REQ_ID_MAP_LINK                          = 0x1c,
+	HIF_REQ_ID_RESET                = 0x0a,
+	HIF_REQ_ID_READ_MIB             = 0x05,
+	HIF_REQ_ID_WRITE_MIB            = 0x06,
+	HIF_REQ_ID_START_SCAN           = 0x07,
+	HIF_REQ_ID_STOP_SCAN            = 0x08,
+	HIF_REQ_ID_TX                   = 0x04,
+	HIF_REQ_ID_JOIN                 = 0x0b,
+	HIF_REQ_ID_SET_PM_MODE          = 0x10,
+	HIF_REQ_ID_SET_BSS_PARAMS       = 0x11,
+	HIF_REQ_ID_ADD_KEY              = 0x0c,
+	HIF_REQ_ID_REMOVE_KEY           = 0x0d,
+	HIF_REQ_ID_EDCA_QUEUE_PARAMS    = 0x13,
+	HIF_REQ_ID_START                = 0x17,
+	HIF_REQ_ID_BEACON_TRANSMIT      = 0x18,
+	HIF_REQ_ID_UPDATE_IE            = 0x1b,
+	HIF_REQ_ID_MAP_LINK             = 0x1c,
 };
 
 enum hif_confirmations_ids {
-	HIF_CNF_ID_RESET                             = 0x0a,
-	HIF_CNF_ID_READ_MIB                          = 0x05,
-	HIF_CNF_ID_WRITE_MIB                         = 0x06,
-	HIF_CNF_ID_START_SCAN                        = 0x07,
-	HIF_CNF_ID_STOP_SCAN                         = 0x08,
-	HIF_CNF_ID_TX                                = 0x04,
-	HIF_CNF_ID_MULTI_TRANSMIT                    = 0x1e,
-	HIF_CNF_ID_JOIN                              = 0x0b,
-	HIF_CNF_ID_SET_PM_MODE                       = 0x10,
-	HIF_CNF_ID_SET_BSS_PARAMS                    = 0x11,
-	HIF_CNF_ID_ADD_KEY                           = 0x0c,
-	HIF_CNF_ID_REMOVE_KEY                        = 0x0d,
-	HIF_CNF_ID_EDCA_QUEUE_PARAMS                 = 0x13,
-	HIF_CNF_ID_START                             = 0x17,
-	HIF_CNF_ID_BEACON_TRANSMIT                   = 0x18,
-	HIF_CNF_ID_UPDATE_IE                         = 0x1b,
-	HIF_CNF_ID_MAP_LINK                          = 0x1c,
+	HIF_CNF_ID_RESET                = 0x0a,
+	HIF_CNF_ID_READ_MIB             = 0x05,
+	HIF_CNF_ID_WRITE_MIB            = 0x06,
+	HIF_CNF_ID_START_SCAN           = 0x07,
+	HIF_CNF_ID_STOP_SCAN            = 0x08,
+	HIF_CNF_ID_TX                   = 0x04,
+	HIF_CNF_ID_MULTI_TRANSMIT       = 0x1e,
+	HIF_CNF_ID_JOIN                 = 0x0b,
+	HIF_CNF_ID_SET_PM_MODE          = 0x10,
+	HIF_CNF_ID_SET_BSS_PARAMS       = 0x11,
+	HIF_CNF_ID_ADD_KEY              = 0x0c,
+	HIF_CNF_ID_REMOVE_KEY           = 0x0d,
+	HIF_CNF_ID_EDCA_QUEUE_PARAMS    = 0x13,
+	HIF_CNF_ID_START                = 0x17,
+	HIF_CNF_ID_BEACON_TRANSMIT      = 0x18,
+	HIF_CNF_ID_UPDATE_IE            = 0x1b,
+	HIF_CNF_ID_MAP_LINK             = 0x1c,
 };
 
 enum hif_indications_ids {
-	HIF_IND_ID_RX                                = 0x84,
-	HIF_IND_ID_SCAN_CMPL                         = 0x86,
-	HIF_IND_ID_JOIN_COMPLETE                     = 0x8f,
-	HIF_IND_ID_SET_PM_MODE_CMPL                  = 0x89,
-	HIF_IND_ID_SUSPEND_RESUME_TX                 = 0x8c,
-	HIF_IND_ID_EVENT                             = 0x85
+	HIF_IND_ID_RX                   = 0x84,
+	HIF_IND_ID_SCAN_CMPL            = 0x86,
+	HIF_IND_ID_JOIN_COMPLETE        = 0x8f,
+	HIF_IND_ID_SET_PM_MODE_CMPL     = 0x89,
+	HIF_IND_ID_SUSPEND_RESUME_TX    = 0x8c,
+	HIF_IND_ID_EVENT                = 0x85
 };
 
 union hif_commands_ids {
@@ -68,27 +66,11 @@ union hif_commands_ids {
 	enum hif_indications_ids indication;
 };
 
-enum hif_status {
-	HIF_STATUS_SUCCESS                         = 0x0,
-	HIF_STATUS_FAILURE                         = 0x1,
-	HIF_INVALID_PARAMETER                      = 0x2,
-	HIF_STATUS_WARNING                         = 0x3,
-	HIF_ERROR_UNSUPPORTED_MSG_ID               = 0x4,
-	HIF_STATUS_DECRYPTFAILURE                  = 0x10,
-	HIF_STATUS_MICFAILURE                      = 0x11,
-	HIF_STATUS_NO_KEY_FOUND                    = 0x12,
-	HIF_STATUS_RETRY_EXCEEDED                  = 0x13,
-	HIF_STATUS_TX_LIFETIME_EXCEEDED            = 0x14,
-	HIF_REQUEUE                                = 0x15,
-	HIF_STATUS_REFUSED                         = 0x16,
-	HIF_STATUS_BUSY                            = 0x17
-};
-
 struct hif_reset_flags {
-	u8    reset_stat:1;
-	u8    reset_all_int:1;
-	u8    reserved1:6;
-	u8    reserved2[3];
+	u8     reset_stat:1;
+	u8     reset_all_int:1;
+	u8     reserved1:6;
+	u8     reserved2[3];
 } __packed;
 
 struct hif_req_reset {
@@ -96,117 +78,101 @@ struct hif_req_reset {
 } __packed;
 
 struct hif_req_read_mib {
-	u16   mib_id;
-	u16   reserved;
+	__le16 mib_id;
+	__le16 reserved;
 } __packed;
 
 struct hif_cnf_read_mib {
-	u32   status;
-	u16   mib_id;
-	u16   length;
-	u8    mib_data[];
+	__le32 status;
+	__le16 mib_id;
+	__le16 length;
+	u8     mib_data[];
 } __packed;
 
 struct hif_req_write_mib {
-	u16   mib_id;
-	u16   length;
-	u8    mib_data[];
+	__le16 mib_id;
+	__le16 length;
+	u8     mib_data[];
 } __packed;
 
 struct hif_cnf_write_mib {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_ie_flags {
-	u8    beacon:1;
-	u8    probe_resp:1;
-	u8    probe_req:1;
-	u8    reserved1:5;
-	u8    reserved2;
+	u8     beacon:1;
+	u8     probe_resp:1;
+	u8     probe_req:1;
+	u8     reserved1:5;
+	u8     reserved2;
 } __packed;
 
 struct hif_ie_tlv {
-	u8    type;
-	u8    length;
-	u8    data[];
+	u8     type;
+	u8     length;
+	u8     data[];
 } __packed;
 
 struct hif_req_update_ie {
 	struct hif_ie_flags ie_flags;
-	u16   num_ies;
+	__le16 num_ies;
 	struct hif_ie_tlv ie[];
 } __packed;
 
 struct hif_cnf_update_ie {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_scan_type {
-	u8    type:1;
-	u8    mode:1;
-	u8    reserved:6;
+	u8     type:1;
+	u8     mode:1;
+	u8     reserved:6;
 } __packed;
 
 struct hif_scan_flags {
-	u8    fbg:1;
-	u8    reserved1:1;
-	u8    pre:1;
-	u8    reserved2:5;
+	u8     fbg:1;
+	u8     reserved1:1;
+	u8     pre:1;
+	u8     reserved2:5;
 } __packed;
 
 struct hif_auto_scan_param {
-	u16   interval;
-	u8    reserved;
+	__le16 interval;
+	u8     reserved;
 	s8     rssi_thr;
 } __packed;
 
 struct hif_ssid_def {
-	u32   ssid_length;
-	u8    ssid[HIF_API_SSID_SIZE];
+	__le32 ssid_length;
+	u8     ssid[HIF_API_SSID_SIZE];
 } __packed;
 
 #define HIF_API_MAX_NB_SSIDS                           2
 #define HIF_API_MAX_NB_CHANNELS                       14
 
-struct hif_req_start_scan {
-	u8    band;
-	struct hif_scan_type scan_type;
-	struct hif_scan_flags scan_flags;
-	u8    max_transmit_rate;
-	struct hif_auto_scan_param auto_scan_param;
-	u8    num_of_probe_requests;
-	u8    probe_delay;
-	u8    num_of_ssids;
-	u8    num_of_channels;
-	u32   min_channel_time;
-	u32   max_channel_time;
-	s32    tx_power_level;
-	u8    ssid_and_channel_lists[];
-} __packed;
-
 struct hif_req_start_scan_alt {
-	u8    band;
+	u8     band;
 	struct hif_scan_type scan_type;
 	struct hif_scan_flags scan_flags;
-	u8    max_transmit_rate;
+	u8     max_transmit_rate;
 	struct hif_auto_scan_param auto_scan_param;
-	u8    num_of_probe_requests;
-	u8    probe_delay;
-	u8    num_of_ssids;
-	u8    num_of_channels;
-	u32   min_channel_time;
-	u32   max_channel_time;
-	s32    tx_power_level;
+	u8     num_of_probe_requests;
+	u8     probe_delay;
+	u8     num_of_ssids;
+	u8     num_of_channels;
+	__le32 min_channel_time;
+	__le32 max_channel_time;
+	__le32 tx_power_level; // signed value
 	struct hif_ssid_def ssid_def[HIF_API_MAX_NB_SSIDS];
-	u8    channel_list[];
+	u8     channel_list[];
 } __packed;
 
 struct hif_cnf_start_scan {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_cnf_stop_scan {
-	u32   status;
+	__le32 status;
 } __packed;
 
 enum hif_pm_mode_status {
@@ -216,10 +182,10 @@ enum hif_pm_mode_status {
 };
 
 struct hif_ind_scan_cmpl {
-	u32   status;
-	u8    pm_mode;
-	u8    num_channels_completed;
-	u16   reserved;
+	__le32 status;
+	u8     pm_mode;
+	u8     num_channels_completed;
+	__le16 reserved;
 } __packed;
 
 enum hif_queue_id {
@@ -241,46 +207,48 @@ enum hif_stbc {
 };
 
 struct hif_queue {
-	u8    queue_id:2;
-	u8    peer_sta_id:4;
-	u8    reserved:2;
+	u8     queue_id:2;
+	u8     peer_sta_id:4;
+	u8     reserved:2;
 } __packed;
 
 struct hif_data_flags {
-	u8    more:1;
-	u8    fc_offset:3;
-	u8    after_dtim:1;
-	u8    reserved:3;
+	u8     more:1;
+	u8     fc_offset:3;
+	u8     after_dtim:1;
+	u8     reserved:3;
 } __packed;
 
 struct hif_tx_flags {
-	u8    start_exp:1;
-	u8    reserved:3;
-	u8    retry_policy_index:4;
+	u8     start_exp:1;
+	u8     reserved:3;
+	u8     retry_policy_index:4;
 } __packed;
 
 struct hif_ht_tx_parameters {
-	u8    frame_format:4;
-	u8    fec_coding:1;
-	u8    short_gi:1;
-	u8    reserved1:1;
-	u8    stbc:1;
-	u8    reserved2;
-	u8    aggregation:1;
-	u8    reserved3:7;
-	u8    reserved4;
+	u8     frame_format:4;
+	u8     fec_coding:1;
+	u8     short_gi:1;
+	u8     reserved1:1;
+	u8     stbc:1;
+	u8     reserved2;
+	u8     aggregation:1;
+	u8     reserved3:7;
+	u8     reserved4;
 } __packed;
 
 struct hif_req_tx {
-	u32   packet_id;
-	u8    max_tx_rate;
+	// packet_id is not interpreted by the device, so it is not necessary to
+	// declare it little endian
+	u32    packet_id;
+	u8     max_tx_rate;
 	struct hif_queue queue_id;
 	struct hif_data_flags data_flags;
 	struct hif_tx_flags tx_flags;
-	u32   reserved;
-	u32   expire_time;
+	__le32 reserved;
+	__le32 expire_time;
 	struct hif_ht_tx_parameters ht_tx_parameters;
-	u8    frame[];
+	u8     frame[];
 } __packed;
 
 enum hif_qos_ackplcy {
@@ -291,26 +259,29 @@ enum hif_qos_ackplcy {
 };
 
 struct hif_tx_result_flags {
-	u8    aggr:1;
-	u8    requeue:1;
-	u8    ack_policy:2;
-	u8    txop_limit:1;
-	u8    reserved1:3;
-	u8    reserved2;
+	u8     aggr:1;
+	u8     requeue:1;
+	u8     ack_policy:2;
+	u8     txop_limit:1;
+	u8     reserved1:3;
+	u8     reserved2;
 } __packed;
 
 struct hif_cnf_tx {
-	u32   status;
-	u32   packet_id;
-	u8    txed_rate;
-	u8    ack_failures;
+	__le32 status;
+	// packet_id is copied from struct hif_req_tx without been interpreted
+	// by the device, so it is not necessary to declare it little endian
+	u32    packet_id;
+	u8     txed_rate;
+	u8     ack_failures;
 	struct hif_tx_result_flags tx_result_flags;
-	u32   media_delay;
-	u32   tx_queue_delay;
+	__le32 media_delay;
+	__le32 tx_queue_delay;
 } __packed;
 
 struct hif_cnf_multi_transmit {
-	u32   num_tx_confs;
+	u8     num_tx_confs;
+	u8     reserved[3];
 	struct hif_cnf_tx   tx_conf_payload[];
 } __packed;
 
@@ -323,147 +294,150 @@ enum hif_ri_flags_encrypt {
 };
 
 struct hif_rx_flags {
-	u8    encryp:3;
-	u8    in_aggr:1;
-	u8    first_aggr:1;
-	u8    last_aggr:1;
-	u8    defrag:1;
-	u8    beacon:1;
-	u8    tim:1;
-	u8    bitmap:1;
-	u8    match_ssid:1;
-	u8    match_bssid:1;
-	u8    more:1;
-	u8    reserved1:1;
-	u8    ht:1;
-	u8    stbc:1;
-	u8    match_uc_addr:1;
-	u8    match_mc_addr:1;
-	u8    match_bc_addr:1;
-	u8    key_type:1;
-	u8    key_index:4;
-	u8    reserved2:1;
-	u8    peer_sta_id:4;
-	u8    reserved3:2;
-	u8    reserved4:1;
+	u8     encryp:3;
+	u8     in_aggr:1;
+	u8     first_aggr:1;
+	u8     last_aggr:1;
+	u8     defrag:1;
+	u8     beacon:1;
+	u8     tim:1;
+	u8     bitmap:1;
+	u8     match_ssid:1;
+	u8     match_bssid:1;
+	u8     more:1;
+	u8     reserved1:1;
+	u8     ht:1;
+	u8     stbc:1;
+	u8     match_uc_addr:1;
+	u8     match_mc_addr:1;
+	u8     match_bc_addr:1;
+	u8     key_type:1;
+	u8     key_index:4;
+	u8     reserved2:1;
+	u8     peer_sta_id:4;
+	u8     reserved3:2;
+	u8     reserved4:1;
 } __packed;
 
 struct hif_ind_rx {
-	u32   status;
-	u16   channel_number;
-	u8    rxed_rate;
-	u8    rcpi_rssi;
+	__le32 status;
+	u8     channel_number;
+	u8     reserved;
+	u8     rxed_rate;
+	u8     rcpi_rssi;
 	struct hif_rx_flags rx_flags;
-	u8    frame[];
+	u8     frame[];
 } __packed;
 
 
 struct hif_req_edca_queue_params {
-	u8    queue_id;
-	u8    reserved1;
-	u8    aifsn;
-	u8    reserved2;
-	u16   cw_min;
-	u16   cw_max;
-	u16   tx_op_limit;
-	u16   allowed_medium_time;
-	u32   reserved3;
+	u8     queue_id;
+	u8     reserved1;
+	u8     aifsn;
+	u8     reserved2;
+	__le16 cw_min;
+	__le16 cw_max;
+	__le16 tx_op_limit;
+	__le16 allowed_medium_time;
+	__le32 reserved3;
 } __packed;
 
 struct hif_cnf_edca_queue_params {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_join_flags {
-	u8    reserved1:2;
-	u8    force_no_beacon:1;
-	u8    force_with_ind:1;
-	u8    reserved2:4;
+	u8     reserved1:2;
+	u8     force_no_beacon:1;
+	u8     force_with_ind:1;
+	u8     reserved2:4;
 } __packed;
 
 struct hif_req_join {
-	u8    infrastructure_bss_mode:1;
-	u8    reserved1:7;
-	u8    band;
-	u16   channel_number;
-	u8    bssid[ETH_ALEN];
-	u16   atim_window;
-	u8    short_preamble:1;
-	u8    reserved2:7;
-	u8    probe_for_join;
-	u8    reserved3;
+	u8     infrastructure_bss_mode:1;
+	u8     reserved1:7;
+	u8     band;
+	u8     channel_number;
+	u8     reserved;
+	u8     bssid[ETH_ALEN];
+	__le16 atim_window;
+	u8     short_preamble:1;
+	u8     reserved2:7;
+	u8     probe_for_join;
+	u8     reserved3;
 	struct hif_join_flags join_flags;
-	u32   ssid_length;
-	u8    ssid[HIF_API_SSID_SIZE];
-	u32   beacon_interval;
-	u32   basic_rate_set;
+	__le32 ssid_length;
+	u8     ssid[HIF_API_SSID_SIZE];
+	__le32 beacon_interval;
+	__le32 basic_rate_set;
 } __packed;
 
 struct hif_cnf_join {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_ind_join_complete {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_bss_flags {
-	u8    lost_count_only:1;
-	u8    reserved:7;
+	u8     lost_count_only:1;
+	u8     reserved:7;
 } __packed;
 
 struct hif_req_set_bss_params {
 	struct hif_bss_flags bss_flags;
-	u8    beacon_lost_count;
-	u16   aid;
-	u32   operational_rate_set;
+	u8     beacon_lost_count;
+	__le16 aid;
+	__le32 operational_rate_set;
 } __packed;
 
 struct hif_cnf_set_bss_params {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_pm_mode {
-	u8    enter_psm:1;
-	u8    reserved:6;
-	u8    fast_psm:1;
+	u8     enter_psm:1;
+	u8     reserved:6;
+	u8     fast_psm:1;
 } __packed;
 
 struct hif_req_set_pm_mode {
 	struct hif_pm_mode pm_mode;
-	u8    fast_psm_idle_period;
-	u8    ap_psm_change_period;
-	u8    min_auto_ps_poll_period;
+	u8     fast_psm_idle_period;
+	u8     ap_psm_change_period;
+	u8     min_auto_ps_poll_period;
 } __packed;
 
 struct hif_cnf_set_pm_mode {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_ind_set_pm_mode_cmpl {
-	u32   status;
-	u8    pm_mode;
-	u8    reserved[3];
+	__le32 status;
+	u8     pm_mode;
+	u8     reserved[3];
 } __packed;
 
 
 struct hif_req_start {
-	u8    mode;
-	u8    band;
-	u16   channel_number;
-	u32   reserved1;
-	u32   beacon_interval;
-	u8    dtim_period;
-	u8    short_preamble:1;
-	u8    reserved2:7;
-	u8    reserved3;
-	u8    ssid_length;
-	u8    ssid[HIF_API_SSID_SIZE];
-	u32   basic_rate_set;
+	u8     mode;
+	u8     band;
+	u8     channel_number;
+	u8     reserved1;
+	__le32 reserved2;
+	__le32 beacon_interval;
+	u8     dtim_period;
+	u8     short_preamble:1;
+	u8     reserved3:7;
+	u8     reserved4;
+	u8     ssid_length;
+	u8     ssid[HIF_API_SSID_SIZE];
+	__le32 basic_rate_set;
 } __packed;
 
 struct hif_cnf_start {
-	u32   status;
+	__le32 status;
 } __packed;
 
 enum hif_beacon {
@@ -472,46 +446,49 @@ enum hif_beacon {
 };
 
 struct hif_req_beacon_transmit {
-	u8    enable_beaconing;
-	u8    reserved[3];
+	u8     enable_beaconing;
+	u8     reserved[3];
 } __packed;
 
 struct hif_cnf_beacon_transmit {
-	u32   status;
+	__le32 status;
 } __packed;
 
+#define HIF_LINK_ID_MAX            14
+#define HIF_LINK_ID_NOT_ASSOCIATED (HIF_LINK_ID_MAX + 1)
+
 enum hif_sta_map_direction {
 	HIF_STA_MAP                       = 0x0,
 	HIF_STA_UNMAP                     = 0x1
 };
 
 struct hif_map_link_flags {
-	u8    map_direction:1;
-	u8    mfpc:1;
-	u8    reserved:6;
+	u8     map_direction:1;
+	u8     mfpc:1;
+	u8     reserved:6;
 } __packed;
 
 struct hif_req_map_link {
-	u8    mac_addr[ETH_ALEN];
+	u8     mac_addr[ETH_ALEN];
 	struct hif_map_link_flags map_link_flags;
-	u8    peer_sta_id;
+	u8     peer_sta_id;
 } __packed;
 
 struct hif_cnf_map_link {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_suspend_resume_flags {
-	u8    resume:1;
-	u8    reserved1:2;
-	u8    bc_mc_only:1;
-	u8    reserved2:4;
-	u8    reserved3;
+	u8     resume:1;
+	u8     reserved1:2;
+	u8     bc_mc_only:1;
+	u8     reserved2:4;
+	u8     reserved3;
 } __packed;
 
 struct hif_ind_suspend_resume_tx {
 	struct hif_suspend_resume_flags suspend_resume_flags;
-	u16   peer_sta_set;
+	__le16 peer_sta_set;
 } __packed;
 
 
@@ -541,102 +518,102 @@ enum hif_key_type {
 };
 
 struct hif_wep_pairwise_key {
-	u8    peer_address[ETH_ALEN];
-	u8    reserved;
-	u8    key_length;
-	u8    key_data[HIF_API_WEP_KEY_DATA_SIZE];
+	u8     peer_address[ETH_ALEN];
+	u8     reserved;
+	u8     key_length;
+	u8     key_data[HIF_API_WEP_KEY_DATA_SIZE];
 } __packed;
 
 struct hif_wep_group_key {
-	u8    key_id;
-	u8    key_length;
-	u8    reserved[2];
-	u8    key_data[HIF_API_WEP_KEY_DATA_SIZE];
+	u8     key_id;
+	u8     key_length;
+	u8     reserved[2];
+	u8     key_data[HIF_API_WEP_KEY_DATA_SIZE];
 } __packed;
 
 struct hif_tkip_pairwise_key {
-	u8    peer_address[ETH_ALEN];
-	u8    reserved[2];
-	u8    tkip_key_data[HIF_API_TKIP_KEY_DATA_SIZE];
-	u8    rx_mic_key[HIF_API_RX_MIC_KEY_SIZE];
-	u8    tx_mic_key[HIF_API_TX_MIC_KEY_SIZE];
+	u8     peer_address[ETH_ALEN];
+	u8     reserved[2];
+	u8     tkip_key_data[HIF_API_TKIP_KEY_DATA_SIZE];
+	u8     rx_mic_key[HIF_API_RX_MIC_KEY_SIZE];
+	u8     tx_mic_key[HIF_API_TX_MIC_KEY_SIZE];
 } __packed;
 
 struct hif_tkip_group_key {
-	u8    tkip_key_data[HIF_API_TKIP_KEY_DATA_SIZE];
-	u8    rx_mic_key[HIF_API_RX_MIC_KEY_SIZE];
-	u8    key_id;
-	u8    reserved[3];
-	u8    rx_sequence_counter[HIF_API_RX_SEQUENCE_COUNTER_SIZE];
+	u8     tkip_key_data[HIF_API_TKIP_KEY_DATA_SIZE];
+	u8     rx_mic_key[HIF_API_RX_MIC_KEY_SIZE];
+	u8     key_id;
+	u8     reserved[3];
+	u8     rx_sequence_counter[HIF_API_RX_SEQUENCE_COUNTER_SIZE];
 } __packed;
 
 struct hif_aes_pairwise_key {
-	u8    peer_address[ETH_ALEN];
-	u8    reserved[2];
-	u8    aes_key_data[HIF_API_AES_KEY_DATA_SIZE];
+	u8     peer_address[ETH_ALEN];
+	u8     reserved[2];
+	u8     aes_key_data[HIF_API_AES_KEY_DATA_SIZE];
 } __packed;
 
 struct hif_aes_group_key {
-	u8    aes_key_data[HIF_API_AES_KEY_DATA_SIZE];
-	u8    key_id;
-	u8    reserved[3];
-	u8    rx_sequence_counter[HIF_API_RX_SEQUENCE_COUNTER_SIZE];
+	u8     aes_key_data[HIF_API_AES_KEY_DATA_SIZE];
+	u8     key_id;
+	u8     reserved[3];
+	u8     rx_sequence_counter[HIF_API_RX_SEQUENCE_COUNTER_SIZE];
 } __packed;
 
 struct hif_wapi_pairwise_key {
-	u8    peer_address[ETH_ALEN];
-	u8    key_id;
-	u8    reserved;
-	u8    wapi_key_data[HIF_API_WAPI_KEY_DATA_SIZE];
-	u8    mic_key_data[HIF_API_MIC_KEY_DATA_SIZE];
+	u8     peer_address[ETH_ALEN];
+	u8     key_id;
+	u8     reserved;
+	u8     wapi_key_data[HIF_API_WAPI_KEY_DATA_SIZE];
+	u8     mic_key_data[HIF_API_MIC_KEY_DATA_SIZE];
 } __packed;
 
 struct hif_wapi_group_key {
-	u8    wapi_key_data[HIF_API_WAPI_KEY_DATA_SIZE];
-	u8    mic_key_data[HIF_API_MIC_KEY_DATA_SIZE];
-	u8    key_id;
-	u8    reserved[3];
+	u8     wapi_key_data[HIF_API_WAPI_KEY_DATA_SIZE];
+	u8     mic_key_data[HIF_API_MIC_KEY_DATA_SIZE];
+	u8     key_id;
+	u8     reserved[3];
 } __packed;
 
 struct hif_igtk_group_key {
-	u8    igtk_key_data[HIF_API_IGTK_KEY_DATA_SIZE];
-	u8    key_id;
-	u8    reserved[3];
-	u8    ipn[HIF_API_IPN_SIZE];
+	u8     igtk_key_data[HIF_API_IGTK_KEY_DATA_SIZE];
+	u8     key_id;
+	u8     reserved[3];
+	u8     ipn[HIF_API_IPN_SIZE];
 } __packed;
 
 union hif_privacy_key_data {
-	struct hif_wep_pairwise_key                       wep_pairwise_key;
-	struct hif_wep_group_key                          wep_group_key;
-	struct hif_tkip_pairwise_key                      tkip_pairwise_key;
-	struct hif_tkip_group_key                         tkip_group_key;
-	struct hif_aes_pairwise_key                       aes_pairwise_key;
-	struct hif_aes_group_key                          aes_group_key;
-	struct hif_wapi_pairwise_key                      wapi_pairwise_key;
-	struct hif_wapi_group_key                         wapi_group_key;
-	struct hif_igtk_group_key                         igtk_group_key;
+	struct hif_wep_pairwise_key  wep_pairwise_key;
+	struct hif_wep_group_key     wep_group_key;
+	struct hif_tkip_pairwise_key tkip_pairwise_key;
+	struct hif_tkip_group_key    tkip_group_key;
+	struct hif_aes_pairwise_key  aes_pairwise_key;
+	struct hif_aes_group_key     aes_group_key;
+	struct hif_wapi_pairwise_key wapi_pairwise_key;
+	struct hif_wapi_group_key    wapi_group_key;
+	struct hif_igtk_group_key    igtk_group_key;
 };
 
 struct hif_req_add_key {
-	u8    type;
-	u8    entry_index;
-	u8    int_id:2;
-	u8    reserved1:6;
-	u8    reserved2;
+	u8     type;
+	u8     entry_index;
+	u8     int_id:2;
+	u8     reserved1:6;
+	u8     reserved2;
 	union hif_privacy_key_data key;
 } __packed;
 
 struct hif_cnf_add_key {
-	u32   status;
+	__le32 status;
 } __packed;
 
 struct hif_req_remove_key {
-	u8    entry_index;
-	u8    reserved[3];
+	u8     entry_index;
+	u8     reserved[3];
 } __packed;
 
 struct hif_cnf_remove_key {
-	u32   status;
+	__le32 status;
 } __packed;
 
 enum hif_event_ind {
@@ -656,13 +633,13 @@ enum hif_ps_mode_error {
 };
 
 union hif_event_data {
-	u8    rcpi_rssi;
-	u32   ps_mode_error;
-	u32   peer_sta_set;
+	u8     rcpi_rssi;
+	__le32 ps_mode_error;
+	__le32 peer_sta_set;
 };
 
 struct hif_ind_event {
-	u32   event_id;
+	__le32 event_id;
 	union hif_event_data event_data;
 } __packed;
 
diff --git a/drivers/staging/wfx/hif_api_general.h b/drivers/staging/wfx/hif_api_general.h
index a069c3a21b4d..dba18a7ae919 100644
--- a/drivers/staging/wfx/hif_api_general.h
+++ b/drivers/staging/wfx/hif_api_general.h
@@ -17,13 +17,13 @@
 #define __packed __attribute__((__packed__))
 #endif
 
-#define API_SSID_SIZE                       32
+#define API_SSID_SIZE             32
 
-#define HIF_ID_IS_INDICATION               0x80
-#define HIF_COUNTER_MAX                    7
+#define HIF_ID_IS_INDICATION      0x80
+#define HIF_COUNTER_MAX           7
 
 struct hif_msg {
-	u16    len;
+	__le16 len;
 	u8     id;
 	u8     reserved:1;
 	u8     interface:2;
@@ -33,239 +33,252 @@ struct hif_msg {
 } __packed;
 
 enum hif_general_requests_ids {
-	HIF_REQ_ID_CONFIGURATION                         = 0x09,
-	HIF_REQ_ID_CONTROL_GPIO                          = 0x26,
-	HIF_REQ_ID_SET_SL_MAC_KEY                        = 0x27,
-	HIF_REQ_ID_SL_EXCHANGE_PUB_KEYS                  = 0x28,
-	HIF_REQ_ID_SL_CONFIGURE                          = 0x29,
-	HIF_REQ_ID_PREVENT_ROLLBACK                      = 0x2a,
-	HIF_REQ_ID_PTA_SETTINGS                          = 0x2b,
-	HIF_REQ_ID_PTA_PRIORITY                          = 0x2c,
-	HIF_REQ_ID_PTA_STATE                             = 0x2d,
-	HIF_REQ_ID_SHUT_DOWN                             = 0x32,
+	HIF_REQ_ID_CONFIGURATION        = 0x09,
+	HIF_REQ_ID_CONTROL_GPIO         = 0x26,
+	HIF_REQ_ID_SET_SL_MAC_KEY       = 0x27,
+	HIF_REQ_ID_SL_EXCHANGE_PUB_KEYS = 0x28,
+	HIF_REQ_ID_SL_CONFIGURE         = 0x29,
+	HIF_REQ_ID_PREVENT_ROLLBACK     = 0x2a,
+	HIF_REQ_ID_PTA_SETTINGS         = 0x2b,
+	HIF_REQ_ID_PTA_PRIORITY         = 0x2c,
+	HIF_REQ_ID_PTA_STATE            = 0x2d,
+	HIF_REQ_ID_SHUT_DOWN            = 0x32,
 };
 
 enum hif_general_confirmations_ids {
-	HIF_CNF_ID_CONFIGURATION                         = 0x09,
-	HIF_CNF_ID_CONTROL_GPIO                          = 0x26,
-	HIF_CNF_ID_SET_SL_MAC_KEY                        = 0x27,
-	HIF_CNF_ID_SL_EXCHANGE_PUB_KEYS                  = 0x28,
-	HIF_CNF_ID_SL_CONFIGURE                          = 0x29,
-	HIF_CNF_ID_PREVENT_ROLLBACK                      = 0x2a,
-	HIF_CNF_ID_PTA_SETTINGS                          = 0x2b,
-	HIF_CNF_ID_PTA_PRIORITY                          = 0x2c,
-	HIF_CNF_ID_PTA_STATE                             = 0x2d,
-	HIF_CNF_ID_SHUT_DOWN                             = 0x32,
+	HIF_CNF_ID_CONFIGURATION        = 0x09,
+	HIF_CNF_ID_CONTROL_GPIO         = 0x26,
+	HIF_CNF_ID_SET_SL_MAC_KEY       = 0x27,
+	HIF_CNF_ID_SL_EXCHANGE_PUB_KEYS = 0x28,
+	HIF_CNF_ID_SL_CONFIGURE         = 0x29,
+	HIF_CNF_ID_PREVENT_ROLLBACK     = 0x2a,
+	HIF_CNF_ID_PTA_SETTINGS         = 0x2b,
+	HIF_CNF_ID_PTA_PRIORITY         = 0x2c,
+	HIF_CNF_ID_PTA_STATE            = 0x2d,
+	HIF_CNF_ID_SHUT_DOWN            = 0x32,
 };
 
 enum hif_general_indications_ids {
-	HIF_IND_ID_EXCEPTION                             = 0xe0,
-	HIF_IND_ID_STARTUP                               = 0xe1,
-	HIF_IND_ID_WAKEUP                                = 0xe2,
-	HIF_IND_ID_GENERIC                               = 0xe3,
-	HIF_IND_ID_ERROR                                 = 0xe4,
-	HIF_IND_ID_SL_EXCHANGE_PUB_KEYS                  = 0xe5
+	HIF_IND_ID_EXCEPTION            = 0xe0,
+	HIF_IND_ID_STARTUP              = 0xe1,
+	HIF_IND_ID_WAKEUP               = 0xe2,
+	HIF_IND_ID_GENERIC              = 0xe3,
+	HIF_IND_ID_ERROR                = 0xe4,
+	HIF_IND_ID_SL_EXCHANGE_PUB_KEYS = 0xe5
 };
 
-enum hif_hi_status {
-	HI_STATUS_SUCCESS                             = 0x0000,
-	HI_STATUS_FAILURE                             = 0x0001,
-	HI_INVALID_PARAMETER                          = 0x0002,
-	HI_STATUS_GPIO_WARNING                        = 0x0003,
-	HI_ERROR_UNSUPPORTED_MSG_ID                   = 0x0004,
-	SL_MAC_KEY_STATUS_SUCCESS                     = 0x005A,
-	SL_MAC_KEY_STATUS_FAILED_KEY_ALREADY_BURNED   = 0x006B,
-	SL_MAC_KEY_STATUS_FAILED_RAM_MODE_NOT_ALLOWED = 0x007C,
-	SL_MAC_KEY_STATUS_FAILED_UNKNOWN_MODE         = 0x008D,
-	SL_PUB_KEY_EXCHANGE_STATUS_SUCCESS            = 0x009E,
-	SL_PUB_KEY_EXCHANGE_STATUS_FAILED             = 0x00AF,
-	PREVENT_ROLLBACK_CNF_SUCCESS                  = 0x1234,
-	PREVENT_ROLLBACK_CNF_WRONG_MAGIC_WORD         = 0x1256
-};
+#define HIF_STATUS_SUCCESS                         (cpu_to_le32(0x0000))
+#define HIF_STATUS_FAIL                            (cpu_to_le32(0x0001))
+#define HIF_STATUS_INVALID_PARAMETER               (cpu_to_le32(0x0002))
+#define HIF_STATUS_WARNING                         (cpu_to_le32(0x0003))
+#define HIF_STATUS_UNKNOWN_REQUEST                 (cpu_to_le32(0x0004))
+#define HIF_STATUS_RX_FAIL_DECRYPT                 (cpu_to_le32(0x0010))
+#define HIF_STATUS_RX_FAIL_MIC                     (cpu_to_le32(0x0011))
+#define HIF_STATUS_RX_FAIL_NO_KEY                  (cpu_to_le32(0x0012))
+#define HIF_STATUS_TX_FAIL_RETRIES                 (cpu_to_le32(0x0013))
+#define HIF_STATUS_TX_FAIL_TIMEOUT                 (cpu_to_le32(0x0014))
+#define HIF_STATUS_TX_FAIL_REQUEUE                 (cpu_to_le32(0x0015))
+#define HIF_STATUS_REFUSED                         (cpu_to_le32(0x0016))
+#define HIF_STATUS_BUSY                            (cpu_to_le32(0x0017))
+#define HIF_STATUS_SLK_SET_KEY_SUCCESS             (cpu_to_le32(0x005A))
+#define HIF_STATUS_SLK_SET_KEY_ALREADY_BURNED      (cpu_to_le32(0x006B))
+#define HIF_STATUS_SLK_SET_KEY_DISALLOWED_MODE     (cpu_to_le32(0x007C))
+#define HIF_STATUS_SLK_SET_KEY_UNKNOWN_MODE        (cpu_to_le32(0x008D))
+#define HIF_STATUS_SLK_NEGO_SUCCESS                (cpu_to_le32(0x009E))
+#define HIF_STATUS_SLK_NEGO_FAILED                 (cpu_to_le32(0x00AF))
+#define HIF_STATUS_ROLLBACK_SUCCESS                (cpu_to_le32(0x1234))
+#define HIF_STATUS_ROLLBACK_FAIL                   (cpu_to_le32(0x1256))
 
 enum hif_api_rate_index {
-	API_RATE_INDEX_B_1MBPS                   = 0,
-	API_RATE_INDEX_B_2MBPS                   = 1,
-	API_RATE_INDEX_B_5P5MBPS                 = 2,
-	API_RATE_INDEX_B_11MBPS                  = 3,
-	API_RATE_INDEX_PBCC_22MBPS               = 4,
-	API_RATE_INDEX_PBCC_33MBPS               = 5,
-	API_RATE_INDEX_G_6MBPS                   = 6,
-	API_RATE_INDEX_G_9MBPS                   = 7,
-	API_RATE_INDEX_G_12MBPS                  = 8,
-	API_RATE_INDEX_G_18MBPS                  = 9,
-	API_RATE_INDEX_G_24MBPS                  = 10,
-	API_RATE_INDEX_G_36MBPS                  = 11,
-	API_RATE_INDEX_G_48MBPS                  = 12,
-	API_RATE_INDEX_G_54MBPS                  = 13,
-	API_RATE_INDEX_N_6P5MBPS                 = 14,
-	API_RATE_INDEX_N_13MBPS                  = 15,
-	API_RATE_INDEX_N_19P5MBPS                = 16,
-	API_RATE_INDEX_N_26MBPS                  = 17,
-	API_RATE_INDEX_N_39MBPS                  = 18,
-	API_RATE_INDEX_N_52MBPS                  = 19,
-	API_RATE_INDEX_N_58P5MBPS                = 20,
-	API_RATE_INDEX_N_65MBPS                  = 21,
-	API_RATE_NUM_ENTRIES                     = 22
+	API_RATE_INDEX_B_1MBPS     = 0,
+	API_RATE_INDEX_B_2MBPS     = 1,
+	API_RATE_INDEX_B_5P5MBPS   = 2,
+	API_RATE_INDEX_B_11MBPS    = 3,
+	API_RATE_INDEX_PBCC_22MBPS = 4,
+	API_RATE_INDEX_PBCC_33MBPS = 5,
+	API_RATE_INDEX_G_6MBPS     = 6,
+	API_RATE_INDEX_G_9MBPS     = 7,
+	API_RATE_INDEX_G_12MBPS    = 8,
+	API_RATE_INDEX_G_18MBPS    = 9,
+	API_RATE_INDEX_G_24MBPS    = 10,
+	API_RATE_INDEX_G_36MBPS    = 11,
+	API_RATE_INDEX_G_48MBPS    = 12,
+	API_RATE_INDEX_G_54MBPS    = 13,
+	API_RATE_INDEX_N_6P5MBPS   = 14,
+	API_RATE_INDEX_N_13MBPS    = 15,
+	API_RATE_INDEX_N_19P5MBPS  = 16,
+	API_RATE_INDEX_N_26MBPS    = 17,
+	API_RATE_INDEX_N_39MBPS    = 18,
+	API_RATE_INDEX_N_52MBPS    = 19,
+	API_RATE_INDEX_N_58P5MBPS  = 20,
+	API_RATE_INDEX_N_65MBPS    = 21,
+	API_RATE_NUM_ENTRIES       = 22
 };
 
 
 enum hif_fw_type {
-	HIF_FW_TYPE_ETF                             = 0x0,
-	HIF_FW_TYPE_WFM                             = 0x1,
-	HIF_FW_TYPE_WSM                             = 0x2
+	HIF_FW_TYPE_ETF  = 0x0,
+	HIF_FW_TYPE_WFM  = 0x1,
+	HIF_FW_TYPE_WSM  = 0x2
 };
 
 struct hif_capabilities {
-	u8    link_mode:2;
-	u8    reserved1:6;
-	u8    reserved2;
-	u8    reserved3;
-	u8    reserved4;
+	u8     link_mode:2;
+	u8     reserved1:6;
+	u8     reserved2;
+	u8     reserved3;
+	u8     reserved4;
 } __packed;
 
 struct hif_otp_regul_sel_mode_info {
-	u8    region_sel_mode:4;
-	u8    reserved:4;
+	u8     region_sel_mode:4;
+	u8     reserved:4;
 } __packed;
 
 struct hif_otp_phy_info {
-	u8    phy1_region:3;
-	u8    phy0_region:3;
-	u8    otp_phy_ver:2;
+	u8     phy1_region:3;
+	u8     phy0_region:3;
+	u8     otp_phy_ver:2;
 } __packed;
 
-#define API_OPN_SIZE                                    14
-#define API_UID_SIZE                                    8
-#define API_DISABLED_CHANNEL_LIST_SIZE                  2
-#define API_FIRMWARE_LABEL_SIZE                         128
-
 struct hif_ind_startup {
-	u32   status;
-	u16   hardware_id;
-	u8    opn[API_OPN_SIZE];
-	u8    uid[API_UID_SIZE];
-	u16   num_inp_ch_bufs;
-	u16   size_inp_ch_buf;
-	u8    num_links_ap;
-	u8    num_interfaces;
-	u8    mac_addr[2][ETH_ALEN];
-	u8    api_version_minor;
-	u8    api_version_major;
+	// As the others, this struct is interpreted as little endian by the
+	// device. However, this struct is also used by the driver. We prefer to
+	// declare it in native order and doing byte swap on reception.
+	__le32 status;
+	u16    hardware_id;
+	u8     opn[14];
+	u8     uid[8];
+	u16    num_inp_ch_bufs;
+	u16    size_inp_ch_buf;
+	u8     num_links_ap;
+	u8     num_interfaces;
+	u8     mac_addr[2][ETH_ALEN];
+	u8     api_version_minor;
+	u8     api_version_major;
 	struct hif_capabilities capabilities;
-	u8    firmware_build;
-	u8    firmware_minor;
-	u8    firmware_major;
-	u8    firmware_type;
-	u8    disabled_channel_list[API_DISABLED_CHANNEL_LIST_SIZE];
+	u8     firmware_build;
+	u8     firmware_minor;
+	u8     firmware_major;
+	u8     firmware_type;
+	u8     disabled_channel_list[2];
 	struct hif_otp_regul_sel_mode_info regul_sel_mode_info;
 	struct hif_otp_phy_info otp_phy_info;
-	u32   supported_rate_mask;
-	u8    firmware_label[API_FIRMWARE_LABEL_SIZE];
+	u32    supported_rate_mask;
+	u8     firmware_label[128];
 } __packed;
 
 struct hif_ind_wakeup {
 } __packed;
 
 struct hif_req_configuration {
-	u16   length;
-	u8    pds_data[];
+	__le16 length;
+	u8     pds_data[];
 } __packed;
 
 struct hif_cnf_configuration {
-	u32   status;
+	__le32 status;
 } __packed;
 
 enum hif_gpio_mode {
-	HIF_GPIO_MODE_D0                            = 0x0,
-	HIF_GPIO_MODE_D1                            = 0x1,
-	HIF_GPIO_MODE_OD0                           = 0x2,
-	HIF_GPIO_MODE_OD1                           = 0x3,
-	HIF_GPIO_MODE_TRISTATE                      = 0x4,
-	HIF_GPIO_MODE_TOGGLE                        = 0x5,
-	HIF_GPIO_MODE_READ                          = 0x6
+	HIF_GPIO_MODE_D0       = 0x0,
+	HIF_GPIO_MODE_D1       = 0x1,
+	HIF_GPIO_MODE_OD0      = 0x2,
+	HIF_GPIO_MODE_OD1      = 0x3,
+	HIF_GPIO_MODE_TRISTATE = 0x4,
+	HIF_GPIO_MODE_TOGGLE   = 0x5,
+	HIF_GPIO_MODE_READ     = 0x6
 };
 
 struct hif_req_control_gpio {
-	u8 gpio_label;
-	u8 gpio_mode;
+	u8     gpio_label;
+	u8     gpio_mode;
 } __packed;
 
-enum hif_gpio_error {
-	HIF_GPIO_ERROR_0                            = 0x0,
-	HIF_GPIO_ERROR_1                            = 0x1,
-	HIF_GPIO_ERROR_2                            = 0x2
-};
-
 struct hif_cnf_control_gpio {
-	u32 status;
-	u32 value;
+	__le32 status;
+	__le32 value;
 } __packed;
 
 enum hif_generic_indication_type {
-	HIF_GENERIC_INDICATION_TYPE_RAW               = 0x0,
-	HIF_GENERIC_INDICATION_TYPE_STRING            = 0x1,
-	HIF_GENERIC_INDICATION_TYPE_RX_STATS          = 0x2
+	HIF_GENERIC_INDICATION_TYPE_RAW                = 0x0,
+	HIF_GENERIC_INDICATION_TYPE_STRING             = 0x1,
+	HIF_GENERIC_INDICATION_TYPE_RX_STATS           = 0x2,
+	HIF_GENERIC_INDICATION_TYPE_TX_POWER_LOOP_INFO = 0x3,
 };
 
 struct hif_rx_stats {
-	u32   nb_rx_frame;
-	u32   nb_crc_frame;
-	u32   per_total;
-	u32   throughput;
-	u32   nb_rx_by_rate[API_RATE_NUM_ENTRIES];
-	u16   per[API_RATE_NUM_ENTRIES];
-	s16    snr[API_RATE_NUM_ENTRIES];
-	s16    rssi[API_RATE_NUM_ENTRIES];
-	s16    cfo[API_RATE_NUM_ENTRIES];
-	u32   date;
-	u32   pwr_clk_freq;
-	u8    is_ext_pwr_clk;
+	__le32 nb_rx_frame;
+	__le32 nb_crc_frame;
+	__le32 per_total;
+	__le32 throughput;
+	__le32 nb_rx_by_rate[API_RATE_NUM_ENTRIES];
+	__le16 per[API_RATE_NUM_ENTRIES];
+	__le16 snr[API_RATE_NUM_ENTRIES];  // signed value
+	__le16 rssi[API_RATE_NUM_ENTRIES]; // signed value
+	__le16 cfo[API_RATE_NUM_ENTRIES];  // signed value
+	__le32 date;
+	__le32 pwr_clk_freq;
+	u8     is_ext_pwr_clk;
 	s8     current_temp;
 } __packed;
 
+struct hif_tx_power_loop_info {
+	__le16 tx_gain_dig;
+	__le16 tx_gain_pa;
+	__le16 target_pout; // signed value
+	__le16 p_estimation; // signed value
+	__le16 vpdet;
+	u8     measurement_index;
+	u8     reserved;
+} __packed;
+
 union hif_indication_data {
-	struct hif_rx_stats                                   rx_stats;
-	u8                                       raw_data[1];
+	struct hif_rx_stats rx_stats;
+	struct hif_tx_power_loop_info tx_power_loop_info;
+	u8     raw_data[1];
 };
 
 struct hif_ind_generic {
-	u32 indication_type;
+	__le32 indication_type;
 	union hif_indication_data indication_data;
 } __packed;
 
-
-#define HIF_EXCEPTION_DATA_SIZE            124
-
-struct hif_ind_exception {
-	u8    data[HIF_EXCEPTION_DATA_SIZE];
-} __packed;
-
-
 enum hif_error {
-	HIF_ERROR_FIRMWARE_ROLLBACK             = 0x0,
-	HIF_ERROR_FIRMWARE_DEBUG_ENABLED        = 0x1,
-	HIF_ERROR_OUTDATED_SESSION_KEY          = 0x2,
-	HIF_ERROR_INVALID_SESSION_KEY           = 0x3,
-	HIF_ERROR_OOR_VOLTAGE                   = 0x4,
-	HIF_ERROR_PDS_VERSION                   = 0x5,
-	HIF_ERROR_OOR_TEMPERATURE               = 0x6,
-	HIF_ERROR_REQ_DURING_KEY_EXCHANGE       = 0x7,
-	HIF_ERROR_MULTI_TX_CNF_SECURELINK       = 0x8,
-	HIF_ERROR_SECURELINK_OVERFLOW           = 0x9,
-	HIF_ERROR_SECURELINK_DECRYPTION         = 0xa
+	HIF_ERROR_FIRMWARE_ROLLBACK           = 0x00,
+	HIF_ERROR_FIRMWARE_DEBUG_ENABLED      = 0x01,
+	HIF_ERROR_SLK_OUTDATED_SESSION_KEY    = 0x02,
+	HIF_ERROR_SLK_SESSION_KEY             = 0x03,
+	HIF_ERROR_OOR_VOLTAGE                 = 0x04,
+	HIF_ERROR_PDS_PAYLOAD                 = 0x05,
+	HIF_ERROR_OOR_TEMPERATURE             = 0x06,
+	HIF_ERROR_SLK_REQ_DURING_KEY_EXCHANGE = 0x07,
+	HIF_ERROR_SLK_MULTI_TX_UNSUPPORTED    = 0x08,
+	HIF_ERROR_SLK_OVERFLOW                = 0x09,
+	HIF_ERROR_SLK_DECRYPTION              = 0x0a,
+	HIF_ERROR_SLK_WRONG_ENCRYPTION_STATE  = 0x0b,
+	HIF_ERROR_HIF_BUS_FREQUENCY_TOO_LOW   = 0x0c,
+	HIF_ERROR_HIF_RX_DATA_TOO_LARGE       = 0x0e,
+	HIF_ERROR_HIF_TX_QUEUE_FULL           = 0x0d,
+	HIF_ERROR_HIF_BUS                     = 0x0f,
+	HIF_ERROR_PDS_TESTFEATURE             = 0x10,
 };
 
 struct hif_ind_error {
-	u32   type;
-	u8    data[];
+	__le32 type;
+	u8     data[];
+} __packed;
+
+struct hif_ind_exception {
+	__le32 type;
+	u8     data[];
 } __packed;
 
 enum hif_secure_link_state {
-	SEC_LINK_UNAVAILABLE                    = 0x0,
-	SEC_LINK_RESERVED                       = 0x1,
-	SEC_LINK_EVAL                           = 0x2,
-	SEC_LINK_ENFORCED                       = 0x3
+	SEC_LINK_UNAVAILABLE = 0x0,
+	SEC_LINK_RESERVED    = 0x1,
+	SEC_LINK_EVAL        = 0x2,
+	SEC_LINK_ENFORCED    = 0x3
 };
 
 enum hif_sl_encryption_type {
@@ -282,156 +295,70 @@ struct hif_sl_msg_hdr {
 
 struct hif_sl_msg {
 	struct hif_sl_msg_hdr hdr;
-	u16        len;
-	u8         payload[];
+	__le16 len;
+	u8     payload[];
 } __packed;
 
-#define AES_CCM_TAG_SIZE     16
+#define AES_CCM_TAG_SIZE          16
 
 struct hif_sl_tag {
-	u8 tag[16];
+	u8     tag[16];
 } __packed;
 
 enum hif_sl_mac_key_dest {
-	SL_MAC_KEY_DEST_OTP                        = 0x78,
-	SL_MAC_KEY_DEST_RAM                        = 0x87
+	SL_MAC_KEY_DEST_OTP = 0x78,
+	SL_MAC_KEY_DEST_RAM = 0x87
 };
 
-#define API_KEY_VALUE_SIZE      32
+#define API_KEY_VALUE_SIZE        32
 
 struct hif_req_set_sl_mac_key {
-	u8    otp_or_ram;
-	u8    key_value[API_KEY_VALUE_SIZE];
+	u8     otp_or_ram;
+	u8     key_value[API_KEY_VALUE_SIZE];
 } __packed;
 
 struct hif_cnf_set_sl_mac_key {
-	u32   status;
+	__le32 status;
 } __packed;
 
-#define API_HOST_PUB_KEY_SIZE                           32
-#define API_HOST_PUB_KEY_MAC_SIZE                       64
-
 enum hif_sl_session_key_alg {
-	HIF_SL_CURVE25519                                = 0x01,
-	HIF_SL_KDF                                       = 0x02
+	HIF_SL_CURVE25519 = 0x01,
+	HIF_SL_KDF        = 0x02
 };
 
+#define API_HOST_PUB_KEY_SIZE     32
+#define API_HOST_PUB_KEY_MAC_SIZE 64
+
 struct hif_req_sl_exchange_pub_keys {
-	u8    algorithm:2;
-	u8    reserved1:6;
-	u8    reserved2[3];
-	u8    host_pub_key[API_HOST_PUB_KEY_SIZE];
-	u8    host_pub_key_mac[API_HOST_PUB_KEY_MAC_SIZE];
+	u8     algorithm:2;
+	u8     reserved1:6;
+	u8     reserved2[3];
+	u8     host_pub_key[API_HOST_PUB_KEY_SIZE];
+	u8     host_pub_key_mac[API_HOST_PUB_KEY_MAC_SIZE];
 } __packed;
 
 struct hif_cnf_sl_exchange_pub_keys {
-	u32   status;
+	__le32 status;
 } __packed;
 
-#define API_NCP_PUB_KEY_SIZE                            32
-#define API_NCP_PUB_KEY_MAC_SIZE                        64
+#define API_NCP_PUB_KEY_SIZE      32
+#define API_NCP_PUB_KEY_MAC_SIZE  64
 
 struct hif_ind_sl_exchange_pub_keys {
-	u32   status;
-	u8    ncp_pub_key[API_NCP_PUB_KEY_SIZE];
-	u8    ncp_pub_key_mac[API_NCP_PUB_KEY_MAC_SIZE];
+	__le32 status;
+	u8     ncp_pub_key[API_NCP_PUB_KEY_SIZE];
+	u8     ncp_pub_key_mac[API_NCP_PUB_KEY_MAC_SIZE];
 } __packed;
 
-#define API_ENCR_BMP_SIZE        32
-
 struct hif_req_sl_configure {
-	u8    encr_bmp[API_ENCR_BMP_SIZE];
-	u8    disable_session_key_protection:1;
-	u8    reserved1:7;
-	u8    reserved2[3];
+	u8     encr_bmp[32];
+	u8     disable_session_key_protection:1;
+	u8     reserved1:7;
+	u8     reserved2[3];
 } __packed;
 
 struct hif_cnf_sl_configure {
-	u32 status;
-} __packed;
-
-struct hif_req_prevent_rollback {
-	u32   magic_word;
-} __packed;
-
-struct hif_cnf_prevent_rollback {
-	u32    status;
-} __packed;
-
-enum hif_pta_mode {
-	PTA_1W_WLAN_MASTER = 0,
-	PTA_1W_COEX_MASTER = 1,
-	PTA_2W             = 2,
-	PTA_3W             = 3,
-	PTA_4W             = 4
-};
-
-enum hif_signal_level {
-	SIGNAL_LOW  = 0,
-	SIGNAL_HIGH = 1
-};
-
-enum hif_coex_type {
-	COEX_TYPE_GENERIC = 0,
-	COEX_TYPE_BLE     = 1
-};
-
-enum hif_grant_state {
-	NO_GRANT = 0,
-	GRANT    = 1
-};
-
-struct hif_req_pta_settings {
-	u8 pta_mode;
-	u8 request_signal_active_level;
-	u8 priority_signal_active_level;
-	u8 freq_signal_active_level;
-	u8 grant_signal_active_level;
-	u8 coex_type;
-	u8 default_grant_state;
-	u8 simultaneous_rx_accesses;
-	u8 priority_sampling_time;
-	u8 tx_rx_sampling_time;
-	u8 freq_sampling_time;
-	u8 grant_valid_time;
-	u8 fem_control_time;
-	u8 first_slot_time;
-	u16 periodic_tx_rx_sampling_time;
-	u16 coex_quota;
-	u16 wlan_quota;
-} __packed;
-
-struct hif_cnf_pta_settings {
-	u32 status;
-} __packed;
-
-enum hif_pta_priority {
-	HIF_PTA_PRIORITY_COEX_MAXIMIZED = 0x00000562,
-	HIF_PTA_PRIORITY_COEX_HIGH      = 0x00000462,
-	HIF_PTA_PRIORITY_BALANCED       = 0x00001461,
-	HIF_PTA_PRIORITY_WLAN_HIGH      = 0x00001851,
-	HIF_PTA_PRIORITY_WLAN_MAXIMIZED = 0x00001A51
-};
-
-struct hif_req_pta_priority {
-	u32 priority;
-} __packed;
-
-struct hif_cnf_pta_priority {
-	u32 status;
-} __packed;
-
-enum hif_pta_state {
-	PTA_OFF = 0,
-	PTA_ON  = 1
-};
-
-struct hif_req_pta_state {
-	u32 pta_state;
-} __packed;
-
-struct hif_cnf_pta_state {
-	u32 status;
+	__le32 status;
 } __packed;
 
 #endif
diff --git a/drivers/staging/wfx/hif_api_mib.h b/drivers/staging/wfx/hif_api_mib.h
index 0c67cd4c1593..6f1434795fa8 100644
--- a/drivers/staging/wfx/hif_api_mib.h
+++ b/drivers/staging/wfx/hif_api_mib.h
@@ -10,175 +10,88 @@
 
 #include "hif_api_general.h"
 
-#define HIF_API_IPV4_ADDRESS_SIZE                       4
-#define HIF_API_IPV6_ADDRESS_SIZE                       16
+#define HIF_API_IPV4_ADDRESS_SIZE 4
+#define HIF_API_IPV6_ADDRESS_SIZE 16
 
 enum hif_mib_ids {
-	HIF_MIB_ID_GL_OPERATIONAL_POWER_MODE       = 0x2000,
-	HIF_MIB_ID_GL_BLOCK_ACK_INFO               = 0x2001,
-	HIF_MIB_ID_GL_SET_MULTI_MSG                = 0x2002,
-	HIF_MIB_ID_CCA_CONFIG                      = 0x2003,
-	HIF_MIB_ID_ETHERTYPE_DATAFRAME_CONDITION   = 0x2010,
-	HIF_MIB_ID_PORT_DATAFRAME_CONDITION        = 0x2011,
-	HIF_MIB_ID_MAGIC_DATAFRAME_CONDITION       = 0x2012,
-	HIF_MIB_ID_MAC_ADDR_DATAFRAME_CONDITION    = 0x2013,
-	HIF_MIB_ID_IPV4_ADDR_DATAFRAME_CONDITION   = 0x2014,
-	HIF_MIB_ID_IPV6_ADDR_DATAFRAME_CONDITION   = 0x2015,
-	HIF_MIB_ID_UC_MC_BC_DATAFRAME_CONDITION    = 0x2016,
-	HIF_MIB_ID_CONFIG_DATA_FILTER              = 0x2017,
-	HIF_MIB_ID_SET_DATA_FILTERING              = 0x2018,
-	HIF_MIB_ID_ARP_IP_ADDRESSES_TABLE          = 0x2019,
-	HIF_MIB_ID_NS_IP_ADDRESSES_TABLE           = 0x201A,
-	HIF_MIB_ID_RX_FILTER                       = 0x201B,
-	HIF_MIB_ID_BEACON_FILTER_TABLE             = 0x201C,
-	HIF_MIB_ID_BEACON_FILTER_ENABLE            = 0x201D,
-	HIF_MIB_ID_GRP_SEQ_COUNTER                 = 0x2030,
-	HIF_MIB_ID_TSF_COUNTER                     = 0x2031,
-	HIF_MIB_ID_STATISTICS_TABLE                = 0x2032,
-	HIF_MIB_ID_COUNTERS_TABLE                  = 0x2033,
-	HIF_MIB_ID_MAX_TX_POWER_LEVEL              = 0x2034,
-	HIF_MIB_ID_EXTENDED_COUNTERS_TABLE         = 0x2035,
-	HIF_MIB_ID_DOT11_MAC_ADDRESS               = 0x2040,
+	HIF_MIB_ID_GL_OPERATIONAL_POWER_MODE        = 0x2000,
+	HIF_MIB_ID_GL_BLOCK_ACK_INFO                = 0x2001,
+	HIF_MIB_ID_GL_SET_MULTI_MSG                 = 0x2002,
+	HIF_MIB_ID_CCA_CONFIG                       = 0x2003,
+	HIF_MIB_ID_ETHERTYPE_DATAFRAME_CONDITION    = 0x2010,
+	HIF_MIB_ID_PORT_DATAFRAME_CONDITION         = 0x2011,
+	HIF_MIB_ID_MAGIC_DATAFRAME_CONDITION        = 0x2012,
+	HIF_MIB_ID_MAC_ADDR_DATAFRAME_CONDITION     = 0x2013,
+	HIF_MIB_ID_IPV4_ADDR_DATAFRAME_CONDITION    = 0x2014,
+	HIF_MIB_ID_IPV6_ADDR_DATAFRAME_CONDITION    = 0x2015,
+	HIF_MIB_ID_UC_MC_BC_DATAFRAME_CONDITION     = 0x2016,
+	HIF_MIB_ID_CONFIG_DATA_FILTER               = 0x2017,
+	HIF_MIB_ID_SET_DATA_FILTERING               = 0x2018,
+	HIF_MIB_ID_ARP_IP_ADDRESSES_TABLE           = 0x2019,
+	HIF_MIB_ID_NS_IP_ADDRESSES_TABLE            = 0x201A,
+	HIF_MIB_ID_RX_FILTER                        = 0x201B,
+	HIF_MIB_ID_BEACON_FILTER_TABLE              = 0x201C,
+	HIF_MIB_ID_BEACON_FILTER_ENABLE             = 0x201D,
+	HIF_MIB_ID_GRP_SEQ_COUNTER                  = 0x2030,
+	HIF_MIB_ID_TSF_COUNTER                      = 0x2031,
+	HIF_MIB_ID_STATISTICS_TABLE                 = 0x2032,
+	HIF_MIB_ID_COUNTERS_TABLE                   = 0x2033,
+	HIF_MIB_ID_MAX_TX_POWER_LEVEL               = 0x2034,
+	HIF_MIB_ID_EXTENDED_COUNTERS_TABLE          = 0x2035,
+	HIF_MIB_ID_DOT11_MAC_ADDRESS                = 0x2040,
 	HIF_MIB_ID_DOT11_MAX_TRANSMIT_MSDU_LIFETIME = 0x2041,
-	HIF_MIB_ID_DOT11_MAX_RECEIVE_LIFETIME      = 0x2042,
-	HIF_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID        = 0x2043,
-	HIF_MIB_ID_DOT11_RTS_THRESHOLD             = 0x2044,
-	HIF_MIB_ID_SLOT_TIME                       = 0x2045,
-	HIF_MIB_ID_CURRENT_TX_POWER_LEVEL          = 0x2046,
-	HIF_MIB_ID_NON_ERP_PROTECTION              = 0x2047,
-	HIF_MIB_ID_TEMPLATE_FRAME                  = 0x2048,
-	HIF_MIB_ID_BEACON_WAKEUP_PERIOD            = 0x2049,
-	HIF_MIB_ID_RCPI_RSSI_THRESHOLD             = 0x204A,
-	HIF_MIB_ID_BLOCK_ACK_POLICY                = 0x204B,
-	HIF_MIB_ID_OVERRIDE_INTERNAL_TX_RATE       = 0x204C,
-	HIF_MIB_ID_SET_ASSOCIATION_MODE            = 0x204D,
-	HIF_MIB_ID_SET_UAPSD_INFORMATION           = 0x204E,
-	HIF_MIB_ID_SET_TX_RATE_RETRY_POLICY        = 0x204F,
-	HIF_MIB_ID_PROTECTED_MGMT_POLICY           = 0x2050,
-	HIF_MIB_ID_SET_HT_PROTECTION               = 0x2051,
-	HIF_MIB_ID_KEEP_ALIVE_PERIOD               = 0x2052,
-	HIF_MIB_ID_ARP_KEEP_ALIVE_PERIOD           = 0x2053,
-	HIF_MIB_ID_INACTIVITY_TIMER                = 0x2054,
-	HIF_MIB_ID_INTERFACE_PROTECTION            = 0x2055,
-	HIF_MIB_ID_BEACON_STATS                    = 0x2056,
+	HIF_MIB_ID_DOT11_MAX_RECEIVE_LIFETIME       = 0x2042,
+	HIF_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID         = 0x2043,
+	HIF_MIB_ID_DOT11_RTS_THRESHOLD              = 0x2044,
+	HIF_MIB_ID_SLOT_TIME                        = 0x2045,
+	HIF_MIB_ID_CURRENT_TX_POWER_LEVEL           = 0x2046,
+	HIF_MIB_ID_NON_ERP_PROTECTION               = 0x2047,
+	HIF_MIB_ID_TEMPLATE_FRAME                   = 0x2048,
+	HIF_MIB_ID_BEACON_WAKEUP_PERIOD             = 0x2049,
+	HIF_MIB_ID_RCPI_RSSI_THRESHOLD              = 0x204A,
+	HIF_MIB_ID_BLOCK_ACK_POLICY                 = 0x204B,
+	HIF_MIB_ID_OVERRIDE_INTERNAL_TX_RATE        = 0x204C,
+	HIF_MIB_ID_SET_ASSOCIATION_MODE             = 0x204D,
+	HIF_MIB_ID_SET_UAPSD_INFORMATION            = 0x204E,
+	HIF_MIB_ID_SET_TX_RATE_RETRY_POLICY         = 0x204F,
+	HIF_MIB_ID_PROTECTED_MGMT_POLICY            = 0x2050,
+	HIF_MIB_ID_SET_HT_PROTECTION                = 0x2051,
+	HIF_MIB_ID_KEEP_ALIVE_PERIOD                = 0x2052,
+	HIF_MIB_ID_ARP_KEEP_ALIVE_PERIOD            = 0x2053,
+	HIF_MIB_ID_INACTIVITY_TIMER                 = 0x2054,
+	HIF_MIB_ID_INTERFACE_PROTECTION             = 0x2055,
+	HIF_MIB_ID_BEACON_STATS                     = 0x2056,
 };
 
-#define HIF_OP_POWER_MODE_MASK                     0xf
-
 enum hif_op_power_mode {
-	HIF_OP_POWER_MODE_ACTIVE                   = 0x0,
-	HIF_OP_POWER_MODE_DOZE                     = 0x1,
-	HIF_OP_POWER_MODE_QUIESCENT                = 0x2
+	HIF_OP_POWER_MODE_ACTIVE    = 0x0,
+	HIF_OP_POWER_MODE_DOZE      = 0x1,
+	HIF_OP_POWER_MODE_QUIESCENT = 0x2
 };
 
 struct hif_mib_gl_operational_power_mode {
-	u8    power_mode:4;
-	u8    reserved1:3;
-	u8    wup_ind_activation:1;
-	u8    reserved2[3];
-} __packed;
-
-struct hif_mib_gl_block_ack_info {
-	u8    rx_buffer_size;
-	u8    rx_max_num_agreements;
-	u8    tx_buffer_size;
-	u8    tx_max_num_agreements;
+	u8     power_mode:4;
+	u8     reserved1:3;
+	u8     wup_ind_activation:1;
+	u8     reserved2[3];
 } __packed;
 
 struct hif_mib_gl_set_multi_msg {
-	u8    enable_multi_tx_conf:1;
-	u8    reserved1:7;
-	u8    reserved2[3];
-} __packed;
-
-enum hif_cca_thr_mode {
-	HIF_CCA_THR_MODE_RELATIVE = 0x0,
-	HIF_CCA_THR_MODE_ABSOLUTE = 0x1
-};
-
-struct hif_mib_gl_cca_config {
-	u8  cca_thr_mode;
-	u8  reserved[3];
-} __packed;
-
-#define MAX_NUMBER_DATA_FILTERS             0xA
-
-#define MAX_NUMBER_IPV4_ADDR_CONDITIONS     0x4
-#define MAX_NUMBER_IPV6_ADDR_CONDITIONS     0x4
-#define MAX_NUMBER_MAC_ADDR_CONDITIONS      0x4
-#define MAX_NUMBER_UC_MC_BC_CONDITIONS      0x4
-#define MAX_NUMBER_ETHER_TYPE_CONDITIONS    0x4
-#define MAX_NUMBER_PORT_CONDITIONS          0x4
-#define MAX_NUMBER_MAGIC_CONDITIONS         0x4
-#define MAX_NUMBER_ARP_CONDITIONS           0x2
-#define MAX_NUMBER_NS_CONDITIONS            0x2
-
-struct hif_mib_ethertype_data_frame_condition {
-	u8    condition_idx;
-	u8    reserved;
-	u16   ether_type;
-} __packed;
-
-enum hif_udp_tcp_protocol {
-	HIF_PROTOCOL_UDP                       = 0x0,
-	HIF_PROTOCOL_TCP                       = 0x1,
-	HIF_PROTOCOL_BOTH_UDP_TCP              = 0x2
-};
-
-enum hif_which_port {
-	HIF_PORT_DST                           = 0x0,
-	HIF_PORT_SRC                           = 0x1,
-	HIF_PORT_SRC_OR_DST                    = 0x2
-};
-
-struct hif_mib_ports_data_frame_condition {
-	u8    condition_idx;
-	u8    protocol;
-	u8    which_port;
-	u8    reserved1;
-	u16   port_number;
-	u8    reserved2[2];
-} __packed;
-
-#define HIF_API_MAGIC_PATTERN_SIZE                 32
-
-struct hif_mib_magic_data_frame_condition {
-	u8    condition_idx;
-	u8    offset;
-	u8    magic_pattern_length;
-	u8    reserved;
-	u8    magic_pattern[HIF_API_MAGIC_PATTERN_SIZE];
+	u8     enable_multi_tx_conf:1;
+	u8     reserved1:7;
+	u8     reserved2[3];
 } __packed;
 
 enum hif_mac_addr_type {
-	HIF_MAC_ADDR_A1                            = 0x0,
-	HIF_MAC_ADDR_A2                            = 0x1,
-	HIF_MAC_ADDR_A3                            = 0x2
+	HIF_MAC_ADDR_A1 = 0x0,
+	HIF_MAC_ADDR_A2 = 0x1,
+	HIF_MAC_ADDR_A3 = 0x2
 };
 
 struct hif_mib_mac_addr_data_frame_condition {
-	u8    condition_idx;
-	u8    address_type;
-	u8    mac_address[ETH_ALEN];
-} __packed;
-
-enum hif_ip_addr_mode {
-	HIF_IP_ADDR_SRC                            = 0x0,
-	HIF_IP_ADDR_DST                            = 0x1
-};
-
-struct hif_mib_ipv4_addr_data_frame_condition {
-	u8    condition_idx;
-	u8    address_mode;
-	u8    reserved[2];
-	u8    i_pv4_address[HIF_API_IPV4_ADDRESS_SIZE];
-} __packed;
-
-struct hif_mib_ipv6_addr_data_frame_condition {
-	u8    condition_idx;
-	u8    address_mode;
-	u8    reserved[2];
-	u8    i_pv6_address[HIF_API_IPV6_ADDRESS_SIZE];
+	u8     condition_idx;
+	u8     address_type;
+	u8     mac_address[ETH_ALEN];
 } __packed;
 
 #define HIF_FILTER_UNICAST   0x1
@@ -186,365 +99,289 @@ struct hif_mib_ipv6_addr_data_frame_condition {
 #define HIF_FILTER_BROADCAST 0x4
 
 struct hif_mib_uc_mc_bc_data_frame_condition {
-	u8    condition_idx;
-	u8    allowed_frames;
-	u8    reserved[2];
+	u8     condition_idx;
+	u8     allowed_frames;
+	u8     reserved[2];
 } __packed;
 
 struct hif_mib_config_data_filter {
-	u8    filter_idx;
-	u8    enable;
-	u8    reserved1[2];
-	u8    eth_type_cond;
-	u8    port_cond;
-	u8    magic_cond;
-	u8    mac_cond;
-	u8    ipv4_cond;
-	u8    ipv6_cond;
-	u8    uc_mc_bc_cond;
-	u8    reserved2;
+	u8     filter_idx;
+	u8     enable;
+	u8     reserved1[2];
+	u8     eth_type_cond;
+	u8     port_cond;
+	u8     magic_cond;
+	u8     mac_cond;
+	u8     ipv4_cond;
+	u8     ipv6_cond;
+	u8     uc_mc_bc_cond;
+	u8     reserved2;
 } __packed;
 
 struct hif_mib_set_data_filtering {
-	u8    invert_matching:1;
-	u8    reserved1:7;
-	u8    enable:1;
-	u8    reserved2:7;
-	u8    reserved3[2];
+	u8     invert_matching:1;
+	u8     reserved1:7;
+	u8     enable:1;
+	u8     reserved2:7;
+	u8     reserved3[2];
 } __packed;
 
 enum hif_arp_ns_frame_treatment {
-	HIF_ARP_NS_FILTERING_DISABLE                  = 0x0,
-	HIF_ARP_NS_FILTERING_ENABLE                   = 0x1,
-	HIF_ARP_NS_REPLY_ENABLE                       = 0x2
+	HIF_ARP_NS_FILTERING_DISABLE = 0x0,
+	HIF_ARP_NS_FILTERING_ENABLE  = 0x1,
+	HIF_ARP_NS_REPLY_ENABLE      = 0x2
 };
 
 struct hif_mib_arp_ip_addr_table {
-	u8    condition_idx;
-	u8    arp_enable;
-	u8    reserved[2];
-	u8    ipv4_address[HIF_API_IPV4_ADDRESS_SIZE];
-} __packed;
-
-struct hif_mib_ns_ip_addr_table {
-	u8    condition_idx;
-	u8    ns_enable;
-	u8    reserved[2];
-	u8    ipv6_address[HIF_API_IPV6_ADDRESS_SIZE];
+	u8     condition_idx;
+	u8     arp_enable;
+	u8     reserved[2];
+	u8     ipv4_address[HIF_API_IPV4_ADDRESS_SIZE];
 } __packed;
 
 struct hif_mib_rx_filter {
-	u8    reserved1:1;
-	u8    bssid_filter:1;
-	u8    reserved2:1;
-	u8    fwd_probe_req:1;
-	u8    keep_alive_filter:1;
-	u8    reserved3:3;
-	u8    reserved4[3];
+	u8     reserved1:1;
+	u8     bssid_filter:1;
+	u8     reserved2:1;
+	u8     fwd_probe_req:1;
+	u8     keep_alive_filter:1;
+	u8     reserved3:3;
+	u8     reserved4[3];
 } __packed;
 
-#define HIF_API_OUI_SIZE                                3
-#define HIF_API_MATCH_DATA_SIZE                         3
-
 struct hif_ie_table_entry {
-	u8    ie_id;
-	u8    has_changed:1;
-	u8    no_longer:1;
-	u8    has_appeared:1;
-	u8    reserved:1;
-	u8    num_match_data:4;
-	u8    oui[HIF_API_OUI_SIZE];
-	u8    match_data[HIF_API_MATCH_DATA_SIZE];
+	u8     ie_id;
+	u8     has_changed:1;
+	u8     no_longer:1;
+	u8     has_appeared:1;
+	u8     reserved:1;
+	u8     num_match_data:4;
+	u8     oui[3];
+	u8     match_data[3];
 } __packed;
 
 struct hif_mib_bcn_filter_table {
-	u32   num_of_info_elmts;
+	__le32 num_of_info_elmts;
 	struct hif_ie_table_entry ie_table[];
 } __packed;
 
 enum hif_beacon_filter {
-	HIF_BEACON_FILTER_DISABLE                  = 0x0,
-	HIF_BEACON_FILTER_ENABLE                   = 0x1,
-	HIF_BEACON_FILTER_AUTO_ERP                 = 0x2
+	HIF_BEACON_FILTER_DISABLE  = 0x0,
+	HIF_BEACON_FILTER_ENABLE   = 0x1,
+	HIF_BEACON_FILTER_AUTO_ERP = 0x2
 };
 
 struct hif_mib_bcn_filter_enable {
-	u32   enable;
-	u32   bcn_count;
-} __packed;
-
-struct hif_mib_group_seq_counter {
-	u32   bits4716;
-	u16   bits1500;
-	u16   reserved;
-} __packed;
-
-struct hif_mib_tsf_counter {
-	u32   tsf_counterlo;
-	u32   tsf_counterhi;
-} __packed;
-
-struct hif_mib_stats_table {
-	s16    latest_snr;
-	u8    latest_rcpi;
-	s8     latest_rssi;
+	__le32 enable;
+	__le32 bcn_count;
 } __packed;
 
 struct hif_mib_extended_count_table {
-	u32   count_plcp_errors;
-	u32   count_fcs_errors;
-	u32   count_tx_packets;
-	u32   count_rx_packets;
-	u32   count_rx_packet_errors;
-	u32   count_rx_decryption_failures;
-	u32   count_rx_mic_failures;
-	u32   count_rx_no_key_failures;
-	u32   count_tx_multicast_frames;
-	u32   count_tx_frames_success;
-	u32   count_tx_frame_failures;
-	u32   count_tx_frames_retried;
-	u32   count_tx_frames_multi_retried;
-	u32   count_rx_frame_duplicates;
-	u32   count_rts_success;
-	u32   count_rts_failures;
-	u32   count_ack_failures;
-	u32   count_rx_multicast_frames;
-	u32   count_rx_frames_success;
-	u32   count_rx_cmacicv_errors;
-	u32   count_rx_cmac_replays;
-	u32   count_rx_mgmt_ccmp_replays;
-	u32   count_rx_bipmic_errors;
-	u32   count_rx_beacon;
-	u32   count_miss_beacon;
-	u32   reserved[15];
+	__le32 count_plcp_errors;
+	__le32 count_fcs_errors;
+	__le32 count_tx_packets;
+	__le32 count_rx_packets;
+	__le32 count_rx_packet_errors;
+	__le32 count_rx_decryption_failures;
+	__le32 count_rx_mic_failures;
+	__le32 count_rx_no_key_failures;
+	__le32 count_tx_multicast_frames;
+	__le32 count_tx_frames_success;
+	__le32 count_tx_frame_failures;
+	__le32 count_tx_frames_retried;
+	__le32 count_tx_frames_multi_retried;
+	__le32 count_rx_frame_duplicates;
+	__le32 count_rts_success;
+	__le32 count_rts_failures;
+	__le32 count_ack_failures;
+	__le32 count_rx_multicast_frames;
+	__le32 count_rx_frames_success;
+	__le32 count_rx_cmacicv_errors;
+	__le32 count_rx_cmac_replays;
+	__le32 count_rx_mgmt_ccmp_replays;
+	__le32 count_rx_bipmic_errors;
+	__le32 count_rx_beacon;
+	__le32 count_miss_beacon;
+	__le32 reserved[15];
 } __packed;
 
 struct hif_mib_count_table {
-	u32   count_plcp_errors;
-	u32   count_fcs_errors;
-	u32   count_tx_packets;
-	u32   count_rx_packets;
-	u32   count_rx_packet_errors;
-	u32   count_rx_decryption_failures;
-	u32   count_rx_mic_failures;
-	u32   count_rx_no_key_failures;
-	u32   count_tx_multicast_frames;
-	u32   count_tx_frames_success;
-	u32   count_tx_frame_failures;
-	u32   count_tx_frames_retried;
-	u32   count_tx_frames_multi_retried;
-	u32   count_rx_frame_duplicates;
-	u32   count_rts_success;
-	u32   count_rts_failures;
-	u32   count_ack_failures;
-	u32   count_rx_multicast_frames;
-	u32   count_rx_frames_success;
-	u32   count_rx_cmacicv_errors;
-	u32   count_rx_cmac_replays;
-	u32   count_rx_mgmt_ccmp_replays;
-	u32   count_rx_bipmic_errors;
-} __packed;
-
-struct hif_mib_max_tx_power_level {
-	s32       max_tx_power_level_rf_port1;
-	s32       max_tx_power_level_rf_port2;
-} __packed;
-
-struct hif_mib_beacon_stats {
-	s32     latest_tbtt_diff;
-	u32    reserved[4];
+	__le32 count_plcp_errors;
+	__le32 count_fcs_errors;
+	__le32 count_tx_packets;
+	__le32 count_rx_packets;
+	__le32 count_rx_packet_errors;
+	__le32 count_rx_decryption_failures;
+	__le32 count_rx_mic_failures;
+	__le32 count_rx_no_key_failures;
+	__le32 count_tx_multicast_frames;
+	__le32 count_tx_frames_success;
+	__le32 count_tx_frame_failures;
+	__le32 count_tx_frames_retried;
+	__le32 count_tx_frames_multi_retried;
+	__le32 count_rx_frame_duplicates;
+	__le32 count_rts_success;
+	__le32 count_rts_failures;
+	__le32 count_ack_failures;
+	__le32 count_rx_multicast_frames;
+	__le32 count_rx_frames_success;
+	__le32 count_rx_cmacicv_errors;
+	__le32 count_rx_cmac_replays;
+	__le32 count_rx_mgmt_ccmp_replays;
+	__le32 count_rx_bipmic_errors;
 } __packed;
 
 struct hif_mib_mac_address {
-	u8    mac_addr[ETH_ALEN];
-	u16   reserved;
-} __packed;
-
-struct hif_mib_dot11_max_transmit_msdu_lifetime {
-	u32   max_life_time;
-} __packed;
-
-struct hif_mib_dot11_max_receive_lifetime {
-	u32   max_life_time;
+	u8     mac_addr[ETH_ALEN];
+	__le16 reserved;
 } __packed;
 
 struct hif_mib_wep_default_key_id {
-	u8    wep_default_key_id;
-	u8    reserved[3];
+	u8     wep_default_key_id;
+	u8     reserved[3];
 } __packed;
 
 struct hif_mib_dot11_rts_threshold {
-	u32   threshold;
+	__le32 threshold;
 } __packed;
 
 struct hif_mib_slot_time {
-	u32   slot_time;
+	__le32 slot_time;
 } __packed;
 
 struct hif_mib_current_tx_power_level {
-	s32   power_level;
+	__le32 power_level; // signed value
 } __packed;
 
 struct hif_mib_non_erp_protection {
-	u8   use_cts_to_self:1;
-	u8   reserved1:7;
-	u8   reserved2[3];
+	u8     use_cts_to_self:1;
+	u8     reserved1:7;
+	u8     reserved2[3];
 } __packed;
 
 enum hif_tmplt {
-	HIF_TMPLT_PRBREQ                           = 0x0,
-	HIF_TMPLT_BCN                              = 0x1,
-	HIF_TMPLT_NULL                             = 0x2,
-	HIF_TMPLT_QOSNUL                           = 0x3,
-	HIF_TMPLT_PSPOLL                           = 0x4,
-	HIF_TMPLT_PRBRES                           = 0x5,
-	HIF_TMPLT_ARP                              = 0x6,
-	HIF_TMPLT_NA                               = 0x7
+	HIF_TMPLT_PRBREQ = 0x0,
+	HIF_TMPLT_BCN    = 0x1,
+	HIF_TMPLT_NULL   = 0x2,
+	HIF_TMPLT_QOSNUL = 0x3,
+	HIF_TMPLT_PSPOLL = 0x4,
+	HIF_TMPLT_PRBRES = 0x5,
+	HIF_TMPLT_ARP    = 0x6,
+	HIF_TMPLT_NA     = 0x7
 };
 
-#define HIF_API_MAX_TEMPLATE_FRAME_SIZE                              700
+#define HIF_API_MAX_TEMPLATE_FRAME_SIZE 700
 
 struct hif_mib_template_frame {
-	u8    frame_type;
-	u8    init_rate:7;
-	u8    mode:1;
-	u16   frame_length;
-	u8    frame[HIF_API_MAX_TEMPLATE_FRAME_SIZE];
+	u8     frame_type;
+	u8     init_rate:7;
+	u8     mode:1;
+	__le16 frame_length;
+	u8     frame[];
 } __packed;
 
 struct hif_mib_beacon_wake_up_period {
-	u8    wakeup_period_min;
-	u8    receive_dtim:1;
-	u8    reserved1:7;
-	u8    wakeup_period_max;
-	u8    reserved2;
+	u8     wakeup_period_min;
+	u8     receive_dtim:1;
+	u8     reserved1:7;
+	u8     wakeup_period_max;
+	u8     reserved2;
 } __packed;
 
 struct hif_mib_rcpi_rssi_threshold {
-	u8    detection:1;
-	u8    rcpi_rssi:1;
-	u8    upperthresh:1;
-	u8    lowerthresh:1;
-	u8    reserved:4;
-	u8    lower_threshold;
-	u8    upper_threshold;
-	u8    rolling_average_count;
+	u8     detection:1;
+	u8     rcpi_rssi:1;
+	u8     upperthresh:1;
+	u8     lowerthresh:1;
+	u8     reserved:4;
+	u8     lower_threshold;
+	u8     upper_threshold;
+	u8     rolling_average_count;
 } __packed;
 
 #define DEFAULT_BA_MAX_RX_BUFFER_SIZE 16
 
 struct hif_mib_block_ack_policy {
-	u8    block_ack_tx_tid_policy;
-	u8    reserved1;
-	u8    block_ack_rx_tid_policy;
-	u8    block_ack_rx_max_buffer_size;
-} __packed;
-
-struct hif_mib_override_int_rate {
-	u8    internal_tx_rate;
-	u8    non_erp_internal_tx_rate;
-	u8    reserved[2];
+	u8     block_ack_tx_tid_policy;
+	u8     reserved1;
+	u8     block_ack_rx_tid_policy;
+	u8     block_ack_rx_max_buffer_size;
 } __packed;
 
 enum hif_mpdu_start_spacing {
-	HIF_MPDU_START_SPACING_NO_RESTRIC          = 0x0,
-	HIF_MPDU_START_SPACING_QUARTER             = 0x1,
-	HIF_MPDU_START_SPACING_HALF                = 0x2,
-	HIF_MPDU_START_SPACING_ONE                 = 0x3,
-	HIF_MPDU_START_SPACING_TWO                 = 0x4,
-	HIF_MPDU_START_SPACING_FOUR                = 0x5,
-	HIF_MPDU_START_SPACING_EIGHT               = 0x6,
-	HIF_MPDU_START_SPACING_SIXTEEN             = 0x7
+	HIF_MPDU_START_SPACING_NO_RESTRIC = 0x0,
+	HIF_MPDU_START_SPACING_QUARTER    = 0x1,
+	HIF_MPDU_START_SPACING_HALF       = 0x2,
+	HIF_MPDU_START_SPACING_ONE        = 0x3,
+	HIF_MPDU_START_SPACING_TWO        = 0x4,
+	HIF_MPDU_START_SPACING_FOUR       = 0x5,
+	HIF_MPDU_START_SPACING_EIGHT      = 0x6,
+	HIF_MPDU_START_SPACING_SIXTEEN    = 0x7
 };
 
 struct hif_mib_set_association_mode {
-	u8    preambtype_use:1;
-	u8    mode:1;
-	u8    rateset:1;
-	u8    spacing:1;
-	u8    reserved1:4;
-	u8    short_preamble:1;
-	u8    reserved2:7;
-	u8    greenfield:1;
-	u8    reserved3:7;
-	u8    mpdu_start_spacing;
-	u32   basic_rate_set;
+	u8     preambtype_use:1;
+	u8     mode:1;
+	u8     rateset:1;
+	u8     spacing:1;
+	u8     reserved1:4;
+	u8     short_preamble:1;
+	u8     reserved2:7;
+	u8     greenfield:1;
+	u8     reserved3:7;
+	u8     mpdu_start_spacing;
+	__le32 basic_rate_set;
 } __packed;
 
 struct hif_mib_set_uapsd_information {
-	u8    trig_bckgrnd:1;
-	u8    trig_be:1;
-	u8    trig_video:1;
-	u8    trig_voice:1;
-	u8    reserved1:4;
-	u8    deliv_bckgrnd:1;
-	u8    deliv_be:1;
-	u8    deliv_video:1;
-	u8    deliv_voice:1;
-	u8    reserved2:4;
-	u16   min_auto_trigger_interval;
-	u16   max_auto_trigger_interval;
-	u16   auto_trigger_step;
+	u8     trig_bckgrnd:1;
+	u8     trig_be:1;
+	u8     trig_video:1;
+	u8     trig_voice:1;
+	u8     reserved1:4;
+	u8     deliv_bckgrnd:1;
+	u8     deliv_be:1;
+	u8     deliv_video:1;
+	u8     deliv_voice:1;
+	u8     reserved2:4;
+	__le16 min_auto_trigger_interval;
+	__le16 max_auto_trigger_interval;
+	__le16 auto_trigger_step;
 } __packed;
 
 struct hif_mib_tx_rate_retry_policy {
-	u8    policy_index;
-	u8    short_retry_count;
-	u8    long_retry_count;
-	u8    first_rate_sel:2;
-	u8    terminate:1;
-	u8    count_init:1;
-	u8    reserved1:4;
-	u8    rate_recovery_count;
-	u8    reserved2[3];
-	u8    rates[12];
+	u8     policy_index;
+	u8     short_retry_count;
+	u8     long_retry_count;
+	u8     first_rate_sel:2;
+	u8     terminate:1;
+	u8     count_init:1;
+	u8     reserved1:4;
+	u8     rate_recovery_count;
+	u8     reserved2[3];
+	u8     rates[12];
 } __packed;
 
-#define HIF_MIB_NUM_TX_RATE_RETRY_POLICIES    15
+#define HIF_TX_RETRY_POLICY_MAX     15
+#define HIF_TX_RETRY_POLICY_INVALID HIF_TX_RETRY_POLICY_MAX
 
 struct hif_mib_set_tx_rate_retry_policy {
-	u8    num_tx_rate_policies;
-	u8    reserved[3];
+	u8     num_tx_rate_policies;
+	u8     reserved[3];
 	struct hif_mib_tx_rate_retry_policy tx_rate_retry_policy[];
 } __packed;
 
 struct hif_mib_protected_mgmt_policy {
-	u8   pmf_enable:1;
-	u8   unpmf_allowed:1;
-	u8   host_enc_auth_frames:1;
-	u8   reserved1:5;
-	u8   reserved2[3];
-} __packed;
-
-struct hif_mib_set_ht_protection {
-	u8   dual_cts_prot:1;
-	u8   reserved1:7;
-	u8   reserved2[3];
+	u8     pmf_enable:1;
+	u8     unpmf_allowed:1;
+	u8     host_enc_auth_frames:1;
+	u8     reserved1:5;
+	u8     reserved2[3];
 } __packed;
 
 struct hif_mib_keep_alive_period {
-	u16   keep_alive_period;
-	u8    reserved[2];
-} __packed;
-
-struct hif_mib_arp_keep_alive_period {
-	u16   arp_keep_alive_period;
-	u8    encr_type;
-	u8    reserved;
-	u8    sender_ipv4_address[HIF_API_IPV4_ADDRESS_SIZE];
-	u8    target_ipv4_address[HIF_API_IPV4_ADDRESS_SIZE];
-} __packed;
-
-struct hif_mib_inactivity_timer {
-	u8    min_active_time;
-	u8    max_active_time;
-	u16   reserved;
-} __packed;
-
-struct hif_mib_interface_protection {
-	u8   use_cts_prot:1;
-	u8   reserved1:7;
-	u8   reserved2[3];
+	__le16 keep_alive_period;
+	u8     reserved[2];
 } __packed;
 
 #endif
diff --git a/drivers/staging/wfx/hif_rx.c b/drivers/staging/wfx/hif_rx.c
index 33c22c5d629d..bb156033d1e1 100644
--- a/drivers/staging/wfx/hif_rx.c
+++ b/drivers/staging/wfx/hif_rx.c
@@ -22,9 +22,9 @@ static int hif_generic_confirm(struct wfx_dev *wdev,
 			       const struct hif_msg *hif, const void *buf)
 {
 	// All confirm messages start with status
-	int status = le32_to_cpu(*((__le32 *) buf));
+	int status = le32_to_cpup((__le32 *)buf);
 	int cmd = hif->id;
-	int len = hif->len - 4; // drop header
+	int len = le16_to_cpu(hif->len) - 4; // drop header
 
 	WARN(!mutex_is_locked(&wdev->hif_cmd.lock), "data locking error");
 
@@ -100,10 +100,10 @@ static int hif_startup_indication(struct wfx_dev *wdev,
 		return -EINVAL;
 	}
 	memcpy(&wdev->hw_caps, body, sizeof(struct hif_ind_startup));
-	le32_to_cpus(&wdev->hw_caps.status);
-	le16_to_cpus(&wdev->hw_caps.hardware_id);
-	le16_to_cpus(&wdev->hw_caps.num_inp_ch_bufs);
-	le16_to_cpus(&wdev->hw_caps.size_inp_ch_buf);
+	le16_to_cpus((__le16 *)&wdev->hw_caps.hardware_id);
+	le16_to_cpus((__le16 *)&wdev->hw_caps.num_inp_ch_bufs);
+	le16_to_cpus((__le16 *)&wdev->hw_caps.size_inp_ch_buf);
+	le32_to_cpus((__le32 *)&wdev->hw_caps.supported_rate_mask);
 
 	complete(&wdev->firmware_ready);
 	return 0;
@@ -127,7 +127,7 @@ static int hif_keys_indication(struct wfx_dev *wdev,
 	u8 pubkey[API_NCP_PUB_KEY_SIZE];
 
 	// SL_PUB_KEY_EXCHANGE_STATUS_SUCCESS is used by legacy secure link
-	if (body->status && body->status != SL_PUB_KEY_EXCHANGE_STATUS_SUCCESS)
+	if (body->status && body->status != HIF_STATUS_SLK_NEGO_SUCCESS)
 		dev_warn(wdev->dev, "secure link negociation error\n");
 	memcpy(pubkey, body->ncp_pub_key, sizeof(pubkey));
 	memreverse(pubkey, sizeof(pubkey));
@@ -158,26 +158,39 @@ static int hif_event_indication(struct wfx_dev *wdev,
 {
 	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
 	const struct hif_ind_event *body = buf;
-	struct wfx_hif_event *event;
-	int first;
+	int type = le32_to_cpu(body->event_id);
+	int cause;
 
-	WARN_ON(!wvif);
-	if (!wvif)
+	if (!wvif) {
+		dev_warn(wdev->dev, "received event for non-existent vif\n");
 		return 0;
+	}
 
-	event = kzalloc(sizeof(*event), GFP_KERNEL);
-	if (!event)
-		return -ENOMEM;
-
-	memcpy(&event->evt, body, sizeof(struct hif_ind_event));
-	spin_lock(&wvif->event_queue_lock);
-	first = list_empty(&wvif->event_queue);
-	list_add_tail(&event->link, &wvif->event_queue);
-	spin_unlock(&wvif->event_queue_lock);
-
-	if (first)
-		schedule_work(&wvif->event_handler_work);
-
+	switch (type) {
+	case HIF_EVENT_IND_RCPI_RSSI:
+		wfx_event_report_rssi(wvif, body->event_data.rcpi_rssi);
+		break;
+	case HIF_EVENT_IND_BSSLOST:
+		schedule_delayed_work(&wvif->beacon_loss_work, 0);
+		break;
+	case HIF_EVENT_IND_BSSREGAINED:
+		cancel_delayed_work(&wvif->beacon_loss_work);
+		dev_dbg(wdev->dev, "ignore BSSREGAINED indication\n");
+		break;
+	case HIF_EVENT_IND_PS_MODE_ERROR:
+		cause = le32_to_cpu(body->event_data.ps_mode_error);
+		dev_warn(wdev->dev, "error while processing power save request: %d\n",
+			 cause);
+		if (cause == HIF_PS_ERROR_AP_NOT_RESP_TO_POLL) {
+			wvif->bss_not_support_ps_poll = true;
+			schedule_work(&wvif->update_pm_work);
+		}
+		break;
+	default:
+		dev_warn(wdev->dev, "unhandled event indication: %.2x\n",
+			 type);
+		break;
+	}
 	return 0;
 }
 
@@ -224,53 +237,21 @@ static int hif_suspend_resume_indication(struct wfx_dev *wdev,
 	struct wfx_vif *wvif = wdev_to_wvif(wdev, hif->interface);
 	const struct hif_ind_suspend_resume_tx *body = buf;
 
-	WARN_ON(!wvif);
-	WARN(!body->suspend_resume_flags.bc_mc_only, "unsupported suspend/resume notification");
-	if (body->suspend_resume_flags.resume)
-		wfx_suspend_resume_mc(wvif, STA_NOTIFY_AWAKE);
-	else
-		wfx_suspend_resume_mc(wvif, STA_NOTIFY_SLEEP);
-
-	return 0;
-}
-
-static int hif_error_indication(struct wfx_dev *wdev,
-				const struct hif_msg *hif, const void *buf)
-{
-	const struct hif_ind_error *body = buf;
-	u8 *pRollback = (u8 *) body->data;
-	u32 *pStatus = (u32 *) body->data;
-
-	switch (body->type) {
-	case HIF_ERROR_FIRMWARE_ROLLBACK:
-		dev_err(wdev->dev,
-			"asynchronous error: firmware rollback error %d\n",
-			*pRollback);
-		break;
-	case HIF_ERROR_FIRMWARE_DEBUG_ENABLED:
-		dev_err(wdev->dev, "asynchronous error: firmware debug feature enabled\n");
-		break;
-	case HIF_ERROR_OUTDATED_SESSION_KEY:
-		dev_err(wdev->dev, "asynchronous error: secure link outdated key: %#.8x\n",
-			*pStatus);
-		break;
-	case HIF_ERROR_INVALID_SESSION_KEY:
-		dev_err(wdev->dev, "asynchronous error: invalid session key\n");
-		break;
-	case HIF_ERROR_OOR_VOLTAGE:
-		dev_err(wdev->dev, "asynchronous error: out-of-range overvoltage: %#.8x\n",
-			*pStatus);
-		break;
-	case HIF_ERROR_PDS_VERSION:
-		dev_err(wdev->dev,
-			"asynchronous error: wrong PDS payload or version: %#.8x\n",
-			*pStatus);
-		break;
-	default:
-		dev_err(wdev->dev, "asynchronous error: unknown (%d)\n",
-			body->type);
-		break;
+	if (body->suspend_resume_flags.bc_mc_only) {
+		WARN_ON(!wvif);
+		if (body->suspend_resume_flags.resume)
+			wfx_suspend_resume_mc(wvif, STA_NOTIFY_AWAKE);
+		else
+			wfx_suspend_resume_mc(wvif, STA_NOTIFY_SLEEP);
+	} else {
+		WARN(body->peer_sta_set, "misunderstood indication");
+		WARN(hif->interface != 2, "misunderstood indication");
+		if (body->suspend_resume_flags.resume)
+			wfx_suspend_hot_dev(wdev, STA_NOTIFY_AWAKE);
+		else
+			wfx_suspend_hot_dev(wdev, STA_NOTIFY_SLEEP);
 	}
+
 	return 0;
 }
 
@@ -278,13 +259,14 @@ static int hif_generic_indication(struct wfx_dev *wdev,
 				  const struct hif_msg *hif, const void *buf)
 {
 	const struct hif_ind_generic *body = buf;
+	int type = le32_to_cpu(body->indication_type);
 
-	switch (body->indication_type) {
+	switch (type) {
 	case HIF_GENERIC_INDICATION_TYPE_RAW:
 		return 0;
 	case HIF_GENERIC_INDICATION_TYPE_STRING:
 		dev_info(wdev->dev, "firmware says: %s\n",
-			 (char *) body->indication_data.raw_data);
+			 (char *)body->indication_data.raw_data);
 		return 0;
 	case HIF_GENERIC_INDICATION_TYPE_RX_STATS:
 		mutex_lock(&wdev->rx_stats_lock);
@@ -296,22 +278,103 @@ static int hif_generic_indication(struct wfx_dev *wdev,
 		       sizeof(wdev->rx_stats));
 		mutex_unlock(&wdev->rx_stats_lock);
 		return 0;
+	case HIF_GENERIC_INDICATION_TYPE_TX_POWER_LOOP_INFO:
+		mutex_lock(&wdev->tx_power_loop_info_lock);
+		memcpy(&wdev->tx_power_loop_info,
+		       &body->indication_data.tx_power_loop_info,
+		       sizeof(wdev->tx_power_loop_info));
+		mutex_unlock(&wdev->tx_power_loop_info_lock);
+		return 0;
 	default:
-		dev_err(wdev->dev,
-			"generic_indication: unknown indication type: %#.8x\n",
-			body->indication_type);
+		dev_err(wdev->dev, "generic_indication: unknown indication type: %#.8x\n",
+			type);
 		return -EIO;
 	}
 }
 
+static const struct {
+	int val;
+	const char *str;
+	bool has_param;
+} hif_errors[] = {
+	{ HIF_ERROR_FIRMWARE_ROLLBACK,
+		"rollback status" },
+	{ HIF_ERROR_FIRMWARE_DEBUG_ENABLED,
+		"debug feature enabled" },
+	{ HIF_ERROR_PDS_PAYLOAD,
+		"PDS version is not supported" },
+	{ HIF_ERROR_PDS_TESTFEATURE,
+		"PDS ask for an unknown test mode" },
+	{ HIF_ERROR_OOR_VOLTAGE,
+		"out-of-range power supply voltage", true },
+	{ HIF_ERROR_OOR_TEMPERATURE,
+		"out-of-range temperature", true },
+	{ HIF_ERROR_SLK_REQ_DURING_KEY_EXCHANGE,
+		"secure link does not expect request during key exchange" },
+	{ HIF_ERROR_SLK_SESSION_KEY,
+		"secure link session key is invalid" },
+	{ HIF_ERROR_SLK_OVERFLOW,
+		"secure link overflow" },
+	{ HIF_ERROR_SLK_WRONG_ENCRYPTION_STATE,
+		"secure link messages list does not match message encryption" },
+	{ HIF_ERROR_HIF_BUS_FREQUENCY_TOO_LOW,
+		"bus clock is too slow (<1kHz)" },
+	{ HIF_ERROR_HIF_RX_DATA_TOO_LARGE,
+		"HIF message too large" },
+	// Following errors only exists in old firmware versions:
+	{ HIF_ERROR_HIF_TX_QUEUE_FULL,
+		"HIF messages queue is full" },
+	{ HIF_ERROR_HIF_BUS,
+		"HIF bus" },
+	{ HIF_ERROR_SLK_MULTI_TX_UNSUPPORTED,
+		"secure link does not support multi-tx confirmations" },
+	{ HIF_ERROR_SLK_OUTDATED_SESSION_KEY,
+		"secure link session key is outdated" },
+	{ HIF_ERROR_SLK_DECRYPTION,
+		"secure link params (nonce or tag) mismatch" },
+};
+
+static int hif_error_indication(struct wfx_dev *wdev,
+				const struct hif_msg *hif, const void *buf)
+{
+	const struct hif_ind_error *body = buf;
+	int type = le32_to_cpu(body->type);
+	int param = (s8)body->data[0];
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hif_errors); i++)
+		if (type == hif_errors[i].val)
+			break;
+	if (i < ARRAY_SIZE(hif_errors))
+		if (hif_errors[i].has_param)
+			dev_err(wdev->dev, "asynchronous error: %s: %d\n",
+				hif_errors[i].str, param);
+		else
+			dev_err(wdev->dev, "asynchronous error: %s\n",
+				hif_errors[i].str);
+	else
+		dev_err(wdev->dev, "asynchronous error: unknown: %08x\n", type);
+	print_hex_dump(KERN_INFO, "hif: ", DUMP_PREFIX_OFFSET,
+		       16, 1, hif, le16_to_cpu(hif->len), false);
+	wdev->chip_frozen = true;
+
+	return 0;
+};
+
 static int hif_exception_indication(struct wfx_dev *wdev,
 				    const struct hif_msg *hif, const void *buf)
 {
-	size_t len = hif->len - 4; // drop header
+	const struct hif_ind_exception *body = buf;
+	int type = le32_to_cpu(body->type);
 
-	dev_err(wdev->dev, "firmware exception\n");
-	print_hex_dump_bytes("Dump: ", DUMP_PREFIX_NONE, buf, len);
-	wdev->chip_frozen = 1;
+	if (type == 4)
+		dev_err(wdev->dev, "firmware assert %d\n",
+			le32_to_cpup((__le32 *)body->data));
+	else
+		dev_err(wdev->dev, "firmware exception\n");
+	print_hex_dump(KERN_INFO, "hif: ", DUMP_PREFIX_OFFSET,
+		       16, 1, hif, le16_to_cpu(hif->len), false);
+	wdev->chip_frozen = true;
 
 	return -1;
 }
diff --git a/drivers/staging/wfx/hif_tx.c b/drivers/staging/wfx/hif_tx.c
index 77bca43aca42..893b67f2f792 100644
--- a/drivers/staging/wfx/hif_tx.c
+++ b/drivers/staging/wfx/hif_tx.c
@@ -23,8 +23,8 @@ void wfx_init_hif_cmd(struct wfx_hif_cmd *hif_cmd)
 	mutex_init(&hif_cmd->key_renew_lock);
 }
 
-static void wfx_fill_header(struct hif_msg *hif, int if_id, unsigned int cmd,
-			    size_t size)
+static void wfx_fill_header(struct hif_msg *hif, int if_id,
+			    unsigned int cmd, size_t size)
 {
 	if (if_id == -1)
 		if_id = 2;
@@ -47,8 +47,8 @@ static void *wfx_alloc_hif(size_t body_len, struct hif_msg **hif)
 		return NULL;
 }
 
-int wfx_cmd_send(struct wfx_dev *wdev, struct hif_msg *request, void *reply,
-		 size_t reply_len, bool async)
+int wfx_cmd_send(struct wfx_dev *wdev, struct hif_msg *request,
+		 void *reply, size_t reply_len, bool async)
 {
 	const char *mib_name = "";
 	const char *mib_sep = "";
@@ -82,6 +82,9 @@ int wfx_cmd_send(struct wfx_dev *wdev, struct hif_msg *request, void *reply,
 	if (async)
 		return 0;
 
+	if (wdev->poll_irq)
+		wfx_bh_poll_irq(wdev);
+
 	ret = wait_for_completion_timeout(&wdev->hif_cmd.done, 1 * HZ);
 	if (!ret) {
 		dev_err(wdev->dev, "chip is abnormally long to answer\n");
@@ -91,7 +94,7 @@ int wfx_cmd_send(struct wfx_dev *wdev, struct hif_msg *request, void *reply,
 	if (!ret) {
 		dev_err(wdev->dev, "chip did not answer\n");
 		wfx_pending_dump_old_frames(wdev, 3000);
-		wdev->chip_frozen = 1;
+		wdev->chip_frozen = true;
 		reinit_completion(&wdev->hif_cmd.done);
 		ret = -ETIMEDOUT;
 	} else {
@@ -103,7 +106,7 @@ int wfx_cmd_send(struct wfx_dev *wdev, struct hif_msg *request, void *reply,
 
 	if (ret &&
 	    (cmd == HIF_REQ_ID_READ_MIB || cmd == HIF_REQ_ID_WRITE_MIB)) {
-		mib_name = get_mib_name(((u16 *) request)[2]);
+		mib_name = get_mib_name(((u16 *)request)[2]);
 		mib_sep = "/";
 	}
 	if (ret < 0)
@@ -128,7 +131,11 @@ int hif_shutdown(struct wfx_dev *wdev)
 	int ret;
 	struct hif_msg *hif;
 
+	if (wdev->chip_frozen)
+		return 0;
 	wfx_alloc_hif(0, &hif);
+	if (!hif)
+		return -ENOMEM;
 	wfx_fill_header(hif, -1, HIF_REQ_ID_SHUT_DOWN, 0);
 	ret = wfx_cmd_send(wdev, hif, NULL, 0, true);
 	// After this command, chip won't reply. Be sure to give enough time to
@@ -152,6 +159,8 @@ int hif_configuration(struct wfx_dev *wdev, const u8 *conf, size_t len)
 	struct hif_msg *hif;
 	struct hif_req_configuration *body = wfx_alloc_hif(buf_len, &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->length = cpu_to_le16(len);
 	memcpy(body->pds_data, conf, len);
 	wfx_fill_header(hif, -1, HIF_REQ_ID_CONFIGURATION, buf_len);
@@ -166,6 +175,8 @@ int hif_reset(struct wfx_vif *wvif, bool reset_stat)
 	struct hif_msg *hif;
 	struct hif_req_reset *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->reset_flags.reset_stat = reset_stat;
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_RESET, sizeof(*body));
 	ret = wfx_cmd_send(wvif->wdev, hif, NULL, 0, false);
@@ -173,8 +184,8 @@ int hif_reset(struct wfx_vif *wvif, bool reset_stat)
 	return ret;
 }
 
-int hif_read_mib(struct wfx_dev *wdev, int vif_id, u16 mib_id, void *val,
-		 size_t val_len)
+int hif_read_mib(struct wfx_dev *wdev, int vif_id, u16 mib_id,
+		 void *val, size_t val_len)
 {
 	int ret;
 	struct hif_msg *hif;
@@ -182,36 +193,43 @@ int hif_read_mib(struct wfx_dev *wdev, int vif_id, u16 mib_id, void *val,
 	struct hif_req_read_mib *body = wfx_alloc_hif(sizeof(*body), &hif);
 	struct hif_cnf_read_mib *reply = kmalloc(buf_len, GFP_KERNEL);
 
+	if (!body || !reply) {
+		ret = -ENOMEM;
+		goto out;
+	}
 	body->mib_id = cpu_to_le16(mib_id);
 	wfx_fill_header(hif, vif_id, HIF_REQ_ID_READ_MIB, sizeof(*body));
 	ret = wfx_cmd_send(wdev, hif, reply, buf_len, false);
 
-	if (!ret && mib_id != reply->mib_id) {
-		dev_warn(wdev->dev,
-			 "%s: confirmation mismatch request\n", __func__);
+	if (!ret && mib_id != le16_to_cpu(reply->mib_id)) {
+		dev_warn(wdev->dev, "%s: confirmation mismatch request\n",
+			 __func__);
 		ret = -EIO;
 	}
 	if (ret == -ENOMEM)
-		dev_err(wdev->dev,
-			"buffer is too small to receive %s (%zu < %d)\n",
-			get_mib_name(mib_id), val_len, reply->length);
+		dev_err(wdev->dev, "buffer is too small to receive %s (%zu < %d)\n",
+			get_mib_name(mib_id), val_len,
+			le16_to_cpu(reply->length));
 	if (!ret)
-		memcpy(val, &reply->mib_data, reply->length);
+		memcpy(val, &reply->mib_data, le16_to_cpu(reply->length));
 	else
 		memset(val, 0xFF, val_len);
+out:
 	kfree(hif);
 	kfree(reply);
 	return ret;
 }
 
-int hif_write_mib(struct wfx_dev *wdev, int vif_id, u16 mib_id, void *val,
-		  size_t val_len)
+int hif_write_mib(struct wfx_dev *wdev, int vif_id, u16 mib_id,
+		  void *val, size_t val_len)
 {
 	int ret;
 	struct hif_msg *hif;
 	int buf_len = sizeof(struct hif_req_write_mib) + val_len;
 	struct hif_req_write_mib *body = wfx_alloc_hif(buf_len, &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->mib_id = cpu_to_le16(mib_id);
 	body->length = cpu_to_le16(val_len);
 	memcpy(&body->mib_data, val, val_len);
@@ -236,6 +254,8 @@ int hif_scan(struct wfx_vif *wvif, struct cfg80211_scan_request *req,
 
 	compiletime_assert(IEEE80211_MAX_SSID_LEN == HIF_API_SSID_SIZE,
 			   "API inconsistency");
+	if (!hif)
+		return -ENOMEM;
 	for (i = 0; i < req->n_ssids; i++) {
 		memcpy(body->ssid_def[i].ssid, req->ssids[i].ssid,
 		       IEEE80211_MAX_SSID_LEN);
@@ -268,7 +288,7 @@ int hif_scan(struct wfx_vif *wvif, struct cfg80211_scan_request *req,
 	tmo_chan_bg = le32_to_cpu(body->max_channel_time) * USEC_PER_TU;
 	tmo_chan_fg = 512 * USEC_PER_TU + body->probe_delay;
 	tmo_chan_fg *= body->num_of_probe_requests;
-	tmo = chan_num * max(tmo_chan_bg, tmo_chan_fg);
+	tmo = chan_num * max(tmo_chan_bg, tmo_chan_fg) + 512 * USEC_PER_TU;
 
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_START_SCAN, buf_len);
 	ret = wfx_cmd_send(wvif->wdev, hif, NULL, 0, false);
@@ -283,6 +303,8 @@ int hif_stop_scan(struct wfx_vif *wvif)
 	// body associated to HIF_REQ_ID_STOP_SCAN is empty
 	wfx_alloc_hif(0, &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_STOP_SCAN, 0);
 	ret = wfx_cmd_send(wvif->wdev, hif, NULL, 0, false);
 	kfree(hif);
@@ -296,19 +318,24 @@ int hif_join(struct wfx_vif *wvif, const struct ieee80211_bss_conf *conf,
 	struct hif_msg *hif;
 	struct hif_req_join *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	WARN_ON(!conf->beacon_int);
 	WARN_ON(!conf->basic_rates);
+	WARN_ON(sizeof(body->ssid) < ssidlen);
+	WARN(!conf->ibss_joined && !ssidlen, "joining an unknown BSS");
+	if (!hif)
+		return -ENOMEM;
 	body->infrastructure_bss_mode = !conf->ibss_joined;
 	body->short_preamble = conf->use_short_preamble;
 	if (channel && channel->flags & IEEE80211_CHAN_NO_IR)
 		body->probe_for_join = 0;
 	else
 		body->probe_for_join = 1;
-	body->channel_number = cpu_to_le16(channel->hw_value);
+	body->channel_number = channel->hw_value;
 	body->beacon_interval = cpu_to_le32(conf->beacon_int);
 	body->basic_rate_set =
 		cpu_to_le32(wfx_rate_mask_to_hw(wvif->wdev, conf->basic_rates));
 	memcpy(body->bssid, conf->bssid, sizeof(body->bssid));
-	if (!conf->ibss_joined && ssid) {
+	if (ssid) {
 		body->ssid_length = cpu_to_le32(ssidlen);
 		memcpy(body->ssid, ssid, ssidlen);
 	}
@@ -318,17 +345,17 @@ int hif_join(struct wfx_vif *wvif, const struct ieee80211_bss_conf *conf,
 	return ret;
 }
 
-int hif_set_bss_params(struct wfx_vif *wvif,
-		       const struct hif_req_set_bss_params *arg)
+int hif_set_bss_params(struct wfx_vif *wvif, int aid, int beacon_lost_count)
 {
 	int ret;
 	struct hif_msg *hif;
-	struct hif_req_set_bss_params *body = wfx_alloc_hif(sizeof(*body),
-							    &hif);
+	struct hif_req_set_bss_params *body =
+		wfx_alloc_hif(sizeof(*body), &hif);
 
-	memcpy(body, arg, sizeof(*body));
-	cpu_to_le16s(&body->aid);
-	cpu_to_le32s(&body->operational_rate_set);
+	if (!hif)
+		return -ENOMEM;
+	body->aid = cpu_to_le16(aid);
+	body->beacon_lost_count = beacon_lost_count;
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_SET_BSS_PARAMS,
 			sizeof(*body));
 	ret = wfx_cmd_send(wvif->wdev, hif, NULL, 0, false);
@@ -343,6 +370,8 @@ int hif_add_key(struct wfx_dev *wdev, const struct hif_req_add_key *arg)
 	// FIXME: only send necessary bits
 	struct hif_req_add_key *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	// FIXME: swap bytes as necessary in body
 	memcpy(body, arg, sizeof(*body));
 	if (wfx_api_older_than(wdev, 1, 5))
@@ -363,6 +392,8 @@ int hif_remove_key(struct wfx_dev *wdev, int idx)
 	struct hif_msg *hif;
 	struct hif_req_remove_key *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->entry_index = idx;
 	wfx_fill_header(hif, -1, HIF_REQ_ID_REMOVE_KEY, sizeof(*body));
 	ret = wfx_cmd_send(wdev, hif, NULL, 0, false);
@@ -382,6 +413,8 @@ int hif_set_edca_queue_params(struct wfx_vif *wvif, u16 queue,
 		return -ENOMEM;
 
 	WARN_ON(arg->aifs > 255);
+	if (!hif)
+		return -ENOMEM;
 	body->aifsn = arg->aifs;
 	body->cw_min = cpu_to_le16(arg->cw_min);
 	body->cw_max = cpu_to_le16(arg->cw_max);
@@ -408,6 +441,8 @@ int hif_set_pm(struct wfx_vif *wvif, bool ps, int dynamic_ps_timeout)
 	if (!body)
 		return -ENOMEM;
 
+	if (!hif)
+		return -ENOMEM;
 	if (ps) {
 		body->pm_mode.enter_psm = 1;
 		// Firmware does not support more than 128ms
@@ -428,9 +463,12 @@ int hif_start(struct wfx_vif *wvif, const struct ieee80211_bss_conf *conf,
 	struct hif_msg *hif;
 	struct hif_req_start *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	WARN_ON(!conf->beacon_int);
+	if (!hif)
+		return -ENOMEM;
 	body->dtim_period = conf->dtim_period;
 	body->short_preamble = conf->use_short_preamble;
-	body->channel_number = cpu_to_le16(channel->hw_value);
+	body->channel_number = channel->hw_value;
 	body->beacon_interval = cpu_to_le32(conf->beacon_int);
 	body->basic_rate_set =
 		cpu_to_le32(wfx_rate_mask_to_hw(wvif->wdev, conf->basic_rates));
@@ -449,6 +487,8 @@ int hif_beacon_transmit(struct wfx_vif *wvif, bool enable)
 	struct hif_req_beacon_transmit *body = wfx_alloc_hif(sizeof(*body),
 							     &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->enable_beaconing = enable ? 1 : 0;
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_BEACON_TRANSMIT,
 			sizeof(*body));
@@ -463,9 +503,11 @@ int hif_map_link(struct wfx_vif *wvif, u8 *mac_addr, int flags, int sta_id)
 	struct hif_msg *hif;
 	struct hif_req_map_link *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	if (mac_addr)
 		ether_addr_copy(body->mac_addr, mac_addr);
-	body->map_link_flags = *(struct hif_map_link_flags *) &flags;
+	body->map_link_flags = *(struct hif_map_link_flags *)&flags;
 	body->peer_sta_id = sta_id;
 	wfx_fill_header(hif, wvif->id, HIF_REQ_ID_MAP_LINK, sizeof(*body));
 	ret = wfx_cmd_send(wvif->wdev, hif, NULL, 0, false);
@@ -480,6 +522,8 @@ int hif_update_ie_beacon(struct wfx_vif *wvif, const u8 *ies, size_t ies_len)
 	int buf_len = sizeof(struct hif_req_update_ie) + ies_len;
 	struct hif_req_update_ie *body = wfx_alloc_hif(buf_len, &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->ie_flags.beacon = 1;
 	body->num_ies = cpu_to_le16(1);
 	memcpy(body->ie, ies, ies_len);
@@ -489,14 +533,16 @@ int hif_update_ie_beacon(struct wfx_vif *wvif, const u8 *ies, size_t ies_len)
 	return ret;
 }
 
-int hif_sl_send_pub_keys(struct wfx_dev *wdev, const uint8_t *pubkey,
-			 const uint8_t *pubkey_hmac)
+int hif_sl_send_pub_keys(struct wfx_dev *wdev,
+			 const u8 *pubkey, const u8 *pubkey_hmac)
 {
 	int ret;
 	struct hif_msg *hif;
 	struct hif_req_sl_exchange_pub_keys *body = wfx_alloc_hif(sizeof(*body),
 								  &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	body->algorithm = HIF_SL_CURVE25519;
 	memcpy(body->host_pub_key, pubkey, sizeof(body->host_pub_key));
 	memcpy(body->host_pub_key_mac, pubkey_hmac,
@@ -506,7 +552,7 @@ int hif_sl_send_pub_keys(struct wfx_dev *wdev, const uint8_t *pubkey,
 	ret = wfx_cmd_send(wdev, hif, NULL, 0, false);
 	kfree(hif);
 	// Compatibility with legacy secure link
-	if (ret == SL_PUB_KEY_EXCHANGE_STATUS_SUCCESS)
+	if (ret == le32_to_cpu(HIF_STATUS_SLK_NEGO_SUCCESS))
 		ret = 0;
 	return ret;
 }
@@ -517,6 +563,8 @@ int hif_sl_config(struct wfx_dev *wdev, const unsigned long *bitmap)
 	struct hif_msg *hif;
 	struct hif_req_sl_configure *body = wfx_alloc_hif(sizeof(*body), &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	memcpy(body->encr_bmp, bitmap, sizeof(body->encr_bmp));
 	wfx_fill_header(hif, -1, HIF_REQ_ID_SL_CONFIGURE, sizeof(*body));
 	ret = wfx_cmd_send(wdev, hif, NULL, 0, false);
@@ -524,21 +572,22 @@ int hif_sl_config(struct wfx_dev *wdev, const unsigned long *bitmap)
 	return ret;
 }
 
-int hif_sl_set_mac_key(struct wfx_dev *wdev, const u8 *slk_key,
-		       int destination)
+int hif_sl_set_mac_key(struct wfx_dev *wdev, const u8 *slk_key, int destination)
 {
 	int ret;
 	struct hif_msg *hif;
 	struct hif_req_set_sl_mac_key *body = wfx_alloc_hif(sizeof(*body),
 							    &hif);
 
+	if (!hif)
+		return -ENOMEM;
 	memcpy(body->key_value, slk_key, sizeof(body->key_value));
 	body->otp_or_ram = destination;
 	wfx_fill_header(hif, -1, HIF_REQ_ID_SET_SL_MAC_KEY, sizeof(*body));
 	ret = wfx_cmd_send(wdev, hif, NULL, 0, false);
 	kfree(hif);
 	// Compatibility with legacy secure link
-	if (ret == SL_MAC_KEY_STATUS_SUCCESS)
+	if (ret == le32_to_cpu(HIF_STATUS_SLK_SET_KEY_SUCCESS))
 		ret = 0;
 	return ret;
 }
diff --git a/drivers/staging/wfx/hif_tx.h b/drivers/staging/wfx/hif_tx.h
index f8520a14c14c..e9eca9330178 100644
--- a/drivers/staging/wfx/hif_tx.h
+++ b/drivers/staging/wfx/hif_tx.h
@@ -10,12 +10,11 @@
 #ifndef WFX_HIF_TX_H
 #define WFX_HIF_TX_H
 
-#include "hif_api_cmd.h"
-
 struct ieee80211_channel;
 struct ieee80211_bss_conf;
 struct ieee80211_tx_queue_params;
 struct cfg80211_scan_request;
+struct hif_req_add_key;
 struct wfx_dev;
 struct wfx_vif;
 
@@ -48,8 +47,7 @@ int hif_stop_scan(struct wfx_vif *wvif);
 int hif_join(struct wfx_vif *wvif, const struct ieee80211_bss_conf *conf,
 	     struct ieee80211_channel *channel, const u8 *ssid, int ssidlen);
 int hif_set_pm(struct wfx_vif *wvif, bool ps, int dynamic_ps_timeout);
-int hif_set_bss_params(struct wfx_vif *wvif,
-		       const struct hif_req_set_bss_params *arg);
+int hif_set_bss_params(struct wfx_vif *wvif, int aid, int beacon_lost_count);
 int hif_add_key(struct wfx_dev *wdev, const struct hif_req_add_key *arg);
 int hif_remove_key(struct wfx_dev *wdev, int idx);
 int hif_set_edca_queue_params(struct wfx_vif *wvif, u16 queue,
@@ -59,8 +57,8 @@ int hif_start(struct wfx_vif *wvif, const struct ieee80211_bss_conf *conf,
 int hif_beacon_transmit(struct wfx_vif *wvif, bool enable);
 int hif_map_link(struct wfx_vif *wvif, u8 *mac_addr, int flags, int sta_id);
 int hif_update_ie_beacon(struct wfx_vif *wvif, const u8 *ies, size_t ies_len);
-int hif_sl_set_mac_key(struct wfx_dev *wdev, const u8 *slk_key,
-		       int destination);
+int hif_sl_set_mac_key(struct wfx_dev *wdev,
+		       const u8 *slk_key, int destination);
 int hif_sl_config(struct wfx_dev *wdev, const unsigned long *bitmap);
 int hif_sl_send_pub_keys(struct wfx_dev *wdev,
 			 const u8 *pubkey, const u8 *pubkey_hmac);
diff --git a/drivers/staging/wfx/hif_tx_mib.c b/drivers/staging/wfx/hif_tx_mib.c
new file mode 100644
index 000000000000..1689cb42acc0
--- /dev/null
+++ b/drivers/staging/wfx/hif_tx_mib.c
@@ -0,0 +1,386 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Implementation of host-to-chip MIBs of WFxxx Split Mac (WSM) API.
+ *
+ * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
+ * Copyright (c) 2010, ST-Ericsson
+ * Copyright (C) 2010, ST-Ericsson SA
+ */
+
+#include <linux/etherdevice.h>
+
+#include "wfx.h"
+#include "hif_tx.h"
+#include "hif_tx_mib.h"
+#include "hif_api_mib.h"
+
+int hif_set_output_power(struct wfx_vif *wvif, int val)
+{
+	struct hif_mib_current_tx_power_level arg = {
+		.power_level = cpu_to_le32(val * 10),
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_CURRENT_TX_POWER_LEVEL,
+			     &arg, sizeof(arg));
+}
+
+int hif_set_beacon_wakeup_period(struct wfx_vif *wvif,
+				 unsigned int dtim_interval,
+				 unsigned int listen_interval)
+{
+	struct hif_mib_beacon_wake_up_period val = {
+		.wakeup_period_min = dtim_interval,
+		.receive_dtim = 0,
+		.wakeup_period_max = listen_interval,
+	};
+
+	if (dtim_interval > 0xFF || listen_interval > 0xFFFF)
+		return -EINVAL;
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_BEACON_WAKEUP_PERIOD,
+			     &val, sizeof(val));
+}
+
+int hif_set_rcpi_rssi_threshold(struct wfx_vif *wvif,
+				int rssi_thold, int rssi_hyst)
+{
+	struct hif_mib_rcpi_rssi_threshold arg = {
+		.rolling_average_count = 8,
+		.detection = 1,
+	};
+
+	if (!rssi_thold && !rssi_hyst) {
+		arg.upperthresh = 1;
+		arg.lowerthresh = 1;
+	} else {
+		arg.upper_threshold = rssi_thold + rssi_hyst;
+		arg.upper_threshold = (arg.upper_threshold + 110) * 2;
+		arg.lower_threshold = rssi_thold;
+		arg.lower_threshold = (arg.lower_threshold + 110) * 2;
+	}
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_RCPI_RSSI_THRESHOLD, &arg, sizeof(arg));
+}
+
+int hif_get_counters_table(struct wfx_dev *wdev, int vif_id,
+			   struct hif_mib_extended_count_table *arg)
+{
+	if (wfx_api_older_than(wdev, 1, 3)) {
+		// extended_count_table is wider than count_table
+		memset(arg, 0xFF, sizeof(*arg));
+		return hif_read_mib(wdev, vif_id, HIF_MIB_ID_COUNTERS_TABLE,
+				    arg, sizeof(struct hif_mib_count_table));
+	} else {
+		return hif_read_mib(wdev, vif_id,
+				    HIF_MIB_ID_EXTENDED_COUNTERS_TABLE, arg,
+				sizeof(struct hif_mib_extended_count_table));
+	}
+}
+
+int hif_set_macaddr(struct wfx_vif *wvif, u8 *mac)
+{
+	struct hif_mib_mac_address msg = { };
+
+	if (mac)
+		ether_addr_copy(msg.mac_addr, mac);
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_DOT11_MAC_ADDRESS,
+			     &msg, sizeof(msg));
+}
+
+int hif_set_rx_filter(struct wfx_vif *wvif,
+		      bool filter_bssid, bool filter_prbreq)
+{
+	struct hif_mib_rx_filter val = { };
+
+	if (filter_bssid)
+		val.bssid_filter = 1;
+	if (!filter_prbreq)
+		val.fwd_probe_req = 1;
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_RX_FILTER,
+			     &val, sizeof(val));
+}
+
+int hif_set_beacon_filter_table(struct wfx_vif *wvif, int tbl_len,
+				const struct hif_ie_table_entry *tbl)
+{
+	int ret;
+	struct hif_mib_bcn_filter_table *val;
+	int buf_len = struct_size(val, ie_table, tbl_len);
+
+	val = kzalloc(buf_len, GFP_KERNEL);
+	if (!val)
+		return -ENOMEM;
+	val->num_of_info_elmts = cpu_to_le32(tbl_len);
+	memcpy(val->ie_table, tbl, tbl_len * sizeof(*tbl));
+	ret = hif_write_mib(wvif->wdev, wvif->id,
+			    HIF_MIB_ID_BEACON_FILTER_TABLE, val, buf_len);
+	kfree(val);
+	return ret;
+}
+
+int hif_beacon_filter_control(struct wfx_vif *wvif,
+			      int enable, int beacon_count)
+{
+	struct hif_mib_bcn_filter_enable arg = {
+		.enable = cpu_to_le32(enable),
+		.bcn_count = cpu_to_le32(beacon_count),
+	};
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_BEACON_FILTER_ENABLE,
+			     &arg, sizeof(arg));
+}
+
+int hif_set_operational_mode(struct wfx_dev *wdev, enum hif_op_power_mode mode)
+{
+	struct hif_mib_gl_operational_power_mode val = {
+		.power_mode = mode,
+		.wup_ind_activation = 1,
+	};
+
+	return hif_write_mib(wdev, -1, HIF_MIB_ID_GL_OPERATIONAL_POWER_MODE,
+			     &val, sizeof(val));
+}
+
+int hif_set_template_frame(struct wfx_vif *wvif, struct sk_buff *skb,
+			   u8 frame_type, int init_rate)
+{
+	struct hif_mib_template_frame *arg;
+
+	WARN(skb->len > HIF_API_MAX_TEMPLATE_FRAME_SIZE, "frame is too big");
+	skb_push(skb, 4);
+	arg = (struct hif_mib_template_frame *)skb->data;
+	skb_pull(skb, 4);
+	arg->init_rate = init_rate;
+	arg->frame_type = frame_type;
+	arg->frame_length = cpu_to_le16(skb->len);
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_TEMPLATE_FRAME,
+			     arg, sizeof(*arg) + skb->len);
+}
+
+int hif_set_mfp(struct wfx_vif *wvif, bool capable, bool required)
+{
+	struct hif_mib_protected_mgmt_policy val = { };
+
+	WARN(required && !capable, "incoherent arguments");
+	if (capable) {
+		val.pmf_enable = 1;
+		val.host_enc_auth_frames = 1;
+	}
+	if (!required)
+		val.unpmf_allowed = 1;
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_PROTECTED_MGMT_POLICY,
+			     &val, sizeof(val));
+}
+
+int hif_set_block_ack_policy(struct wfx_vif *wvif,
+			     u8 tx_tid_policy, u8 rx_tid_policy)
+{
+	struct hif_mib_block_ack_policy val = {
+		.block_ack_tx_tid_policy = tx_tid_policy,
+		.block_ack_rx_tid_policy = rx_tid_policy,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_BLOCK_ACK_POLICY,
+			     &val, sizeof(val));
+}
+
+int hif_set_association_mode(struct wfx_vif *wvif,
+			     struct ieee80211_bss_conf *info)
+{
+	struct ieee80211_sta *sta = NULL;
+	struct hif_mib_set_association_mode val = {
+		.preambtype_use = 1,
+		.mode = 1,
+		.spacing = 1,
+		.short_preamble = info->use_short_preamble,
+	};
+
+	rcu_read_lock(); // protect sta
+	if (info->bssid && !info->ibss_joined)
+		sta = ieee80211_find_sta(wvif->vif, info->bssid);
+
+	// FIXME: it is strange to not retrieve all information from bss_info
+	if (sta && sta->ht_cap.ht_supported) {
+		val.mpdu_start_spacing = sta->ht_cap.ampdu_density;
+		if (!(info->ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
+			val.greenfield = !!(sta->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD);
+	}
+	rcu_read_unlock();
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_SET_ASSOCIATION_MODE, &val, sizeof(val));
+}
+
+int hif_set_tx_rate_retry_policy(struct wfx_vif *wvif,
+				 int policy_index, u8 *rates)
+{
+	struct hif_mib_set_tx_rate_retry_policy *arg;
+	size_t size = struct_size(arg, tx_rate_retry_policy, 1);
+	int ret;
+
+	arg = kzalloc(size, GFP_KERNEL);
+	if (!arg)
+		return -ENOMEM;
+	arg->num_tx_rate_policies = 1;
+	arg->tx_rate_retry_policy[0].policy_index = policy_index;
+	arg->tx_rate_retry_policy[0].short_retry_count = 255;
+	arg->tx_rate_retry_policy[0].long_retry_count = 255;
+	arg->tx_rate_retry_policy[0].first_rate_sel = 1;
+	arg->tx_rate_retry_policy[0].terminate = 1;
+	arg->tx_rate_retry_policy[0].count_init = 1;
+	memcpy(&arg->tx_rate_retry_policy[0].rates, rates,
+	       sizeof(arg->tx_rate_retry_policy[0].rates));
+	ret = hif_write_mib(wvif->wdev, wvif->id,
+			    HIF_MIB_ID_SET_TX_RATE_RETRY_POLICY, arg, size);
+	kfree(arg);
+	return ret;
+}
+
+int hif_set_mac_addr_condition(struct wfx_vif *wvif,
+			       int idx, const u8 *mac_addr)
+{
+	struct hif_mib_mac_addr_data_frame_condition val = {
+		.condition_idx = idx,
+		.address_type = HIF_MAC_ADDR_A1,
+	};
+
+	ether_addr_copy(val.mac_address, mac_addr);
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_MAC_ADDR_DATAFRAME_CONDITION,
+			     &val, sizeof(val));
+}
+
+int hif_set_uc_mc_bc_condition(struct wfx_vif *wvif, int idx, u8 allowed_frames)
+{
+	struct hif_mib_uc_mc_bc_data_frame_condition val = {
+		.condition_idx = idx,
+		.allowed_frames = allowed_frames,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_UC_MC_BC_DATAFRAME_CONDITION,
+			     &val, sizeof(val));
+}
+
+int hif_set_config_data_filter(struct wfx_vif *wvif, bool enable, int idx,
+			       int mac_filters, int frames_types_filters)
+{
+	struct hif_mib_config_data_filter val = {
+		.enable = enable,
+		.filter_idx = idx,
+		.mac_cond = mac_filters,
+		.uc_mc_bc_cond = frames_types_filters,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_CONFIG_DATA_FILTER, &val, sizeof(val));
+}
+
+int hif_set_data_filtering(struct wfx_vif *wvif, bool enable, bool invert)
+{
+	struct hif_mib_set_data_filtering val = {
+		.enable = enable,
+		.invert_matching = invert,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_SET_DATA_FILTERING, &val, sizeof(val));
+}
+
+int hif_keep_alive_period(struct wfx_vif *wvif, int period)
+{
+	struct hif_mib_keep_alive_period arg = {
+		.keep_alive_period = cpu_to_le16(period),
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_KEEP_ALIVE_PERIOD,
+			     &arg, sizeof(arg));
+};
+
+int hif_set_arp_ipv4_filter(struct wfx_vif *wvif, int idx, __be32 *addr)
+{
+	struct hif_mib_arp_ip_addr_table arg = {
+		.condition_idx = idx,
+		.arp_enable = HIF_ARP_NS_FILTERING_DISABLE,
+	};
+
+	if (addr) {
+		// Caution: type of addr is __be32
+		memcpy(arg.ipv4_address, addr, sizeof(arg.ipv4_address));
+		arg.arp_enable = HIF_ARP_NS_FILTERING_ENABLE;
+	}
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_ARP_IP_ADDRESSES_TABLE,
+			     &arg, sizeof(arg));
+}
+
+int hif_use_multi_tx_conf(struct wfx_dev *wdev, bool enable)
+{
+	struct hif_mib_gl_set_multi_msg arg = {
+		.enable_multi_tx_conf = enable,
+	};
+
+	return hif_write_mib(wdev, -1, HIF_MIB_ID_GL_SET_MULTI_MSG,
+			     &arg, sizeof(arg));
+}
+
+int hif_set_uapsd_info(struct wfx_vif *wvif, unsigned long val)
+{
+	struct hif_mib_set_uapsd_information arg = { };
+
+	if (val & BIT(IEEE80211_AC_VO))
+		arg.trig_voice = 1;
+	if (val & BIT(IEEE80211_AC_VI))
+		arg.trig_video = 1;
+	if (val & BIT(IEEE80211_AC_BE))
+		arg.trig_be = 1;
+	if (val & BIT(IEEE80211_AC_BK))
+		arg.trig_bckgrnd = 1;
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_SET_UAPSD_INFORMATION,
+			     &arg, sizeof(arg));
+}
+
+int hif_erp_use_protection(struct wfx_vif *wvif, bool enable)
+{
+	struct hif_mib_non_erp_protection arg = {
+		.use_cts_to_self = enable,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_NON_ERP_PROTECTION, &arg, sizeof(arg));
+}
+
+int hif_slot_time(struct wfx_vif *wvif, int val)
+{
+	struct hif_mib_slot_time arg = {
+		.slot_time = cpu_to_le32(val),
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_SLOT_TIME,
+			     &arg, sizeof(arg));
+}
+
+int hif_wep_default_key_id(struct wfx_vif *wvif, int val)
+{
+	struct hif_mib_wep_default_key_id arg = {
+		.wep_default_key_id = val,
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
+			     &arg, sizeof(arg));
+}
+
+int hif_rts_threshold(struct wfx_vif *wvif, int val)
+{
+	struct hif_mib_dot11_rts_threshold arg = {
+		.threshold = cpu_to_le32(val >= 0 ? val : 0xFFFF),
+	};
+
+	return hif_write_mib(wvif->wdev, wvif->id,
+			     HIF_MIB_ID_DOT11_RTS_THRESHOLD, &arg, sizeof(arg));
+}
diff --git a/drivers/staging/wfx/hif_tx_mib.h b/drivers/staging/wfx/hif_tx_mib.h
index 26b1406f9f6c..86683de7de7c 100644
--- a/drivers/staging/wfx/hif_tx_mib.h
+++ b/drivers/staging/wfx/hif_tx_mib.h
@@ -9,398 +9,48 @@
 #ifndef WFX_HIF_TX_MIB_H
 #define WFX_HIF_TX_MIB_H
 
-#include <linux/etherdevice.h>
-
-#include "wfx.h"
-#include "hif_tx.h"
-#include "hif_api_mib.h"
-
-static inline int hif_set_output_power(struct wfx_vif *wvif, int val)
-{
-	struct hif_mib_current_tx_power_level arg = {
-		.power_level = cpu_to_le32(val * 10),
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_CURRENT_TX_POWER_LEVEL,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_set_beacon_wakeup_period(struct wfx_vif *wvif,
-					       unsigned int dtim_interval,
-					       unsigned int listen_interval)
-{
-	struct hif_mib_beacon_wake_up_period val = {
-		.wakeup_period_min = dtim_interval,
-		.receive_dtim = 0,
-		.wakeup_period_max = cpu_to_le16(listen_interval),
-	};
-
-	if (dtim_interval > 0xFF || listen_interval > 0xFFFF)
-		return -EINVAL;
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_BEACON_WAKEUP_PERIOD,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_rcpi_rssi_threshold(struct wfx_vif *wvif,
-					      int rssi_thold, int rssi_hyst)
-{
-	struct hif_mib_rcpi_rssi_threshold arg = {
-		.rolling_average_count = 8,
-		.detection = 1,
-	};
-
-	if (!rssi_thold && !rssi_hyst) {
-		arg.upperthresh = 1;
-		arg.lowerthresh = 1;
-	} else {
-		arg.upper_threshold = rssi_thold + rssi_hyst;
-		arg.upper_threshold = (arg.upper_threshold + 110) * 2;
-		arg.lower_threshold = rssi_thold;
-		arg.lower_threshold = (arg.lower_threshold + 110) * 2;
-	}
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_RCPI_RSSI_THRESHOLD, &arg, sizeof(arg));
-}
-
-static inline int hif_get_counters_table(struct wfx_dev *wdev,
-					 struct hif_mib_extended_count_table *arg)
-{
-	if (wfx_api_older_than(wdev, 1, 3)) {
-		// extended_count_table is wider than count_table
-		memset(arg, 0xFF, sizeof(*arg));
-		return hif_read_mib(wdev, 0, HIF_MIB_ID_COUNTERS_TABLE,
-				    arg, sizeof(struct hif_mib_count_table));
-	} else {
-		return hif_read_mib(wdev, 0,
-				    HIF_MIB_ID_EXTENDED_COUNTERS_TABLE, arg,
-				sizeof(struct hif_mib_extended_count_table));
-	}
-}
-
-static inline int hif_set_macaddr(struct wfx_vif *wvif, u8 *mac)
-{
-	struct hif_mib_mac_address msg = { };
-
-	if (mac)
-		ether_addr_copy(msg.mac_addr, mac);
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_DOT11_MAC_ADDRESS,
-			     &msg, sizeof(msg));
-}
-
-static inline int hif_set_rx_filter(struct wfx_vif *wvif, bool filter_bssid,
-				    bool fwd_probe_req)
-{
-	struct hif_mib_rx_filter val = { };
-
-	if (filter_bssid)
-		val.bssid_filter = 1;
-	if (fwd_probe_req)
-		val.fwd_probe_req = 1;
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_RX_FILTER,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_beacon_filter_table(struct wfx_vif *wvif,
-					      int tbl_len,
-					      struct hif_ie_table_entry *tbl)
-{
-	int ret;
-	struct hif_mib_bcn_filter_table *val;
-	int buf_len = struct_size(val, ie_table, tbl_len);
-
-	val = kzalloc(buf_len, GFP_KERNEL);
-	if (!val)
-		return -ENOMEM;
-	val->num_of_info_elmts = cpu_to_le32(tbl_len);
-	memcpy(val->ie_table, tbl, tbl_len * sizeof(*tbl));
-	ret = hif_write_mib(wvif->wdev, wvif->id,
-			    HIF_MIB_ID_BEACON_FILTER_TABLE, val, buf_len);
-	kfree(val);
-	return ret;
-}
-
-static inline int hif_beacon_filter_control(struct wfx_vif *wvif,
-					    int enable, int beacon_count)
-{
-	struct hif_mib_bcn_filter_enable arg = {
-		.enable = cpu_to_le32(enable),
-		.bcn_count = cpu_to_le32(beacon_count),
-	};
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_BEACON_FILTER_ENABLE,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_set_operational_mode(struct wfx_dev *wdev,
-					   enum hif_op_power_mode mode)
-{
-	struct hif_mib_gl_operational_power_mode val = {
-		.power_mode = mode,
-		.wup_ind_activation = 1,
-	};
-
-	return hif_write_mib(wdev, -1, HIF_MIB_ID_GL_OPERATIONAL_POWER_MODE,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_template_frame(struct wfx_vif *wvif,
-					 struct sk_buff *skb,
-					 u8 frame_type, int init_rate)
-{
-	struct hif_mib_template_frame *arg;
-
-	skb_push(skb, 4);
-	arg = (struct hif_mib_template_frame *)skb->data;
-	skb_pull(skb, 4);
-	arg->init_rate = init_rate;
-	arg->frame_type = frame_type;
-	arg->frame_length = cpu_to_le16(skb->len);
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_TEMPLATE_FRAME,
-			     arg, sizeof(*arg));
-}
-
-static inline int hif_set_mfp(struct wfx_vif *wvif, bool capable, bool required)
-{
-	struct hif_mib_protected_mgmt_policy val = { };
-
-	WARN(required && !capable, "incoherent arguments");
-	if (capable) {
-		val.pmf_enable = 1;
-		val.host_enc_auth_frames = 1;
-	}
-	if (!required)
-		val.unpmf_allowed = 1;
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_PROTECTED_MGMT_POLICY,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_block_ack_policy(struct wfx_vif *wvif,
-					   u8 tx_tid_policy, u8 rx_tid_policy)
-{
-	struct hif_mib_block_ack_policy val = {
-		.block_ack_tx_tid_policy = tx_tid_policy,
-		.block_ack_rx_tid_policy = rx_tid_policy,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_BLOCK_ACK_POLICY,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_association_mode(struct wfx_vif *wvif,
-					   struct ieee80211_bss_conf *info)
-{
-	int basic_rates = wfx_rate_mask_to_hw(wvif->wdev, info->basic_rates);
-	struct ieee80211_sta *sta = NULL;
-	struct hif_mib_set_association_mode val = {
-		.preambtype_use = 1,
-		.mode = 1,
-		.rateset = 1,
-		.spacing = 1,
-		.short_preamble = info->use_short_preamble,
-		.basic_rate_set = cpu_to_le32(basic_rates)
-	};
-
-	rcu_read_lock(); // protect sta
-	if (info->bssid && !info->ibss_joined)
-		sta = ieee80211_find_sta(wvif->vif, info->bssid);
-
-	// FIXME: it is strange to not retrieve all information from bss_info
-	if (sta && sta->ht_cap.ht_supported) {
-		val.mpdu_start_spacing = sta->ht_cap.ampdu_density;
-		if (!(info->ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
-			val.greenfield = !!(sta->ht_cap.cap & IEEE80211_HT_CAP_GRN_FLD);
-	}
-	rcu_read_unlock();
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_SET_ASSOCIATION_MODE, &val, sizeof(val));
-}
-
-static inline int hif_set_tx_rate_retry_policy(struct wfx_vif *wvif,
-					       int policy_index, uint8_t *rates)
-{
-	struct hif_mib_set_tx_rate_retry_policy *arg;
-	size_t size = struct_size(arg, tx_rate_retry_policy, 1);
-	int ret;
-
-	arg = kzalloc(size, GFP_KERNEL);
-	arg->num_tx_rate_policies = 1;
-	arg->tx_rate_retry_policy[0].policy_index = policy_index;
-	arg->tx_rate_retry_policy[0].short_retry_count = 255;
-	arg->tx_rate_retry_policy[0].long_retry_count = 255;
-	arg->tx_rate_retry_policy[0].first_rate_sel = 1;
-	arg->tx_rate_retry_policy[0].terminate = 1;
-	arg->tx_rate_retry_policy[0].count_init = 1;
-	memcpy(&arg->tx_rate_retry_policy[0].rates, rates,
-	       sizeof(arg->tx_rate_retry_policy[0].rates));
-	ret = hif_write_mib(wvif->wdev, wvif->id,
-			    HIF_MIB_ID_SET_TX_RATE_RETRY_POLICY, arg, size);
-	kfree(arg);
-	return ret;
-}
-
-static inline int hif_set_mac_addr_condition(struct wfx_vif *wvif,
-					     int idx, const u8 *mac_addr)
-{
-	struct hif_mib_mac_addr_data_frame_condition val = {
-		.condition_idx = idx,
-		.address_type = HIF_MAC_ADDR_A1,
-	};
-
-	ether_addr_copy(val.mac_address, mac_addr);
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_MAC_ADDR_DATAFRAME_CONDITION,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_uc_mc_bc_condition(struct wfx_vif *wvif,
-					     int idx, u8 allowed_frames)
-{
-	struct hif_mib_uc_mc_bc_data_frame_condition val = {
-		.condition_idx = idx,
-		.allowed_frames = allowed_frames,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_UC_MC_BC_DATAFRAME_CONDITION,
-			     &val, sizeof(val));
-}
-
-static inline int hif_set_config_data_filter(struct wfx_vif *wvif, bool enable,
-					     int idx, int mac_filters,
-					     int frames_types_filters)
-{
-	struct hif_mib_config_data_filter val = {
-		.enable = enable,
-		.filter_idx = idx,
-		.mac_cond = mac_filters,
-		.uc_mc_bc_cond = frames_types_filters,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_CONFIG_DATA_FILTER, &val, sizeof(val));
-}
-
-static inline int hif_set_data_filtering(struct wfx_vif *wvif,
-					 bool enable, bool invert)
-{
-	struct hif_mib_set_data_filtering val = {
-		.enable = enable,
-		.invert_matching = invert,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_SET_DATA_FILTERING, &val, sizeof(val));
-}
-
-static inline int hif_keep_alive_period(struct wfx_vif *wvif, int period)
-{
-	struct hif_mib_keep_alive_period arg = {
-		.keep_alive_period = cpu_to_le16(period),
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_KEEP_ALIVE_PERIOD,
-			     &arg, sizeof(arg));
-};
-
-static inline int hif_set_arp_ipv4_filter(struct wfx_vif *wvif, int idx,
-					  __be32 *addr)
-{
-	struct hif_mib_arp_ip_addr_table arg = {
-		.condition_idx = idx,
-		.arp_enable = HIF_ARP_NS_FILTERING_DISABLE,
-	};
-
-	if (addr) {
-		// Caution: type of addr is __be32
-		memcpy(arg.ipv4_address, addr, sizeof(arg.ipv4_address));
-		arg.arp_enable = HIF_ARP_NS_FILTERING_ENABLE;
-	}
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_ARP_IP_ADDRESSES_TABLE,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_use_multi_tx_conf(struct wfx_dev *wdev, bool enable)
-{
-	struct hif_mib_gl_set_multi_msg arg = {
-		.enable_multi_tx_conf = enable,
-	};
-
-	return hif_write_mib(wdev, -1, HIF_MIB_ID_GL_SET_MULTI_MSG,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_set_uapsd_info(struct wfx_vif *wvif, unsigned long val)
-{
-	struct hif_mib_set_uapsd_information arg = { };
-
-	if (val & BIT(IEEE80211_AC_VO))
-		arg.trig_voice = 1;
-	if (val & BIT(IEEE80211_AC_VI))
-		arg.trig_video = 1;
-	if (val & BIT(IEEE80211_AC_BE))
-		arg.trig_be = 1;
-	if (val & BIT(IEEE80211_AC_BK))
-		arg.trig_bckgrnd = 1;
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_SET_UAPSD_INFORMATION,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_erp_use_protection(struct wfx_vif *wvif, bool enable)
-{
-	struct hif_mib_non_erp_protection arg = {
-		.use_cts_to_self = enable,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_NON_ERP_PROTECTION, &arg, sizeof(arg));
-}
-
-static inline int hif_slot_time(struct wfx_vif *wvif, int val)
-{
-	struct hif_mib_slot_time arg = {
-		.slot_time = cpu_to_le32(val),
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_SLOT_TIME,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_dual_cts_protection(struct wfx_vif *wvif, bool enable)
-{
-	struct hif_mib_set_ht_protection arg = {
-		.dual_cts_prot = enable,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id, HIF_MIB_ID_SET_HT_PROTECTION,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_wep_default_key_id(struct wfx_vif *wvif, int val)
-{
-	struct hif_mib_wep_default_key_id arg = {
-		.wep_default_key_id = val,
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
-			     &arg, sizeof(arg));
-}
-
-static inline int hif_rts_threshold(struct wfx_vif *wvif, int val)
-{
-	struct hif_mib_dot11_rts_threshold arg = {
-		.threshold = cpu_to_le32(val >= 0 ? val : 0xFFFF),
-	};
-
-	return hif_write_mib(wvif->wdev, wvif->id,
-			     HIF_MIB_ID_DOT11_RTS_THRESHOLD, &arg, sizeof(arg));
-}
+struct wfx_vif;
+struct sk_buff;
+
+int hif_set_output_power(struct wfx_vif *wvif, int val);
+int hif_set_beacon_wakeup_period(struct wfx_vif *wvif,
+				 unsigned int dtim_interval,
+				 unsigned int listen_interval);
+int hif_set_rcpi_rssi_threshold(struct wfx_vif *wvif,
+				int rssi_thold, int rssi_hyst);
+int hif_get_counters_table(struct wfx_dev *wdev, int vif_id,
+			   struct hif_mib_extended_count_table *arg);
+int hif_set_macaddr(struct wfx_vif *wvif, u8 *mac);
+int hif_set_rx_filter(struct wfx_vif *wvif,
+		      bool filter_bssid, bool fwd_probe_req);
+int hif_set_beacon_filter_table(struct wfx_vif *wvif, int tbl_len,
+				const struct hif_ie_table_entry *tbl);
+int hif_beacon_filter_control(struct wfx_vif *wvif,
+			      int enable, int beacon_count);
+int hif_set_operational_mode(struct wfx_dev *wdev, enum hif_op_power_mode mode);
+int hif_set_template_frame(struct wfx_vif *wvif, struct sk_buff *skb,
+			   u8 frame_type, int init_rate);
+int hif_set_mfp(struct wfx_vif *wvif, bool capable, bool required);
+int hif_set_block_ack_policy(struct wfx_vif *wvif,
+			     u8 tx_tid_policy, u8 rx_tid_policy);
+int hif_set_association_mode(struct wfx_vif *wvif,
+			     struct ieee80211_bss_conf *info);
+int hif_set_tx_rate_retry_policy(struct wfx_vif *wvif,
+				 int policy_index, u8 *rates);
+int hif_set_mac_addr_condition(struct wfx_vif *wvif,
+			       int idx, const u8 *mac_addr);
+int hif_set_uc_mc_bc_condition(struct wfx_vif *wvif,
+			       int idx, u8 allowed_frames);
+int hif_set_config_data_filter(struct wfx_vif *wvif, bool enable, int idx,
+			       int mac_filters, int frames_types_filters);
+int hif_set_data_filtering(struct wfx_vif *wvif, bool enable, bool invert);
+int hif_keep_alive_period(struct wfx_vif *wvif, int period);
+int hif_set_arp_ipv4_filter(struct wfx_vif *wvif, int idx, __be32 *addr);
+int hif_use_multi_tx_conf(struct wfx_dev *wdev, bool enable);
+int hif_set_uapsd_info(struct wfx_vif *wvif, unsigned long val);
+int hif_erp_use_protection(struct wfx_vif *wvif, bool enable);
+int hif_slot_time(struct wfx_vif *wvif, int val);
+int hif_wep_default_key_id(struct wfx_vif *wvif, int val);
+int hif_rts_threshold(struct wfx_vif *wvif, int val);
 
 #endif
diff --git a/drivers/staging/wfx/hwio.c b/drivers/staging/wfx/hwio.c
index d3a141d95a0e..777217cdf9a7 100644
--- a/drivers/staging/wfx/hwio.c
+++ b/drivers/staging/wfx/hwio.c
@@ -106,8 +106,8 @@ err:
 	return ret;
 }
 
-static int indirect_read(struct wfx_dev *wdev, int reg, u32 addr, void *buf,
-			 size_t len)
+static int indirect_read(struct wfx_dev *wdev, int reg, u32 addr,
+			 void *buf, size_t len)
 {
 	int ret;
 	int i;
@@ -195,8 +195,8 @@ static int indirect_write_locked(struct wfx_dev *wdev, int reg, u32 addr,
 	return ret;
 }
 
-static int indirect_read32_locked(struct wfx_dev *wdev, int reg, u32 addr,
-				  u32 *val)
+static int indirect_read32_locked(struct wfx_dev *wdev, int reg,
+				  u32 addr, u32 *val)
 {
 	int ret;
 	__le32 *tmp = kmalloc(sizeof(u32), GFP_KERNEL);
@@ -205,15 +205,15 @@ static int indirect_read32_locked(struct wfx_dev *wdev, int reg, u32 addr,
 		return -ENOMEM;
 	wdev->hwbus_ops->lock(wdev->hwbus_priv);
 	ret = indirect_read(wdev, reg, addr, tmp, sizeof(u32));
-	*val = cpu_to_le32(*tmp);
+	*val = le32_to_cpu(*tmp);
 	_trace_io_ind_read32(reg, addr, *val);
 	wdev->hwbus_ops->unlock(wdev->hwbus_priv);
 	kfree(tmp);
 	return ret;
 }
 
-static int indirect_write32_locked(struct wfx_dev *wdev, int reg, u32 addr,
-				   u32 val)
+static int indirect_write32_locked(struct wfx_dev *wdev, int reg,
+				   u32 addr, u32 val)
 {
 	int ret;
 	__le32 *tmp = kmalloc(sizeof(u32), GFP_KERNEL);
@@ -233,7 +233,7 @@ int wfx_data_read(struct wfx_dev *wdev, void *buf, size_t len)
 {
 	int ret;
 
-	WARN((long) buf & 3, "%s: unaligned buffer", __func__);
+	WARN((long)buf & 3, "%s: unaligned buffer", __func__);
 	wdev->hwbus_ops->lock(wdev->hwbus_priv);
 	ret = wdev->hwbus_ops->copy_from_io(wdev->hwbus_priv,
 					    WFX_REG_IN_OUT_QUEUE, buf, len);
@@ -249,7 +249,7 @@ int wfx_data_write(struct wfx_dev *wdev, const void *buf, size_t len)
 {
 	int ret;
 
-	WARN((long) buf & 3, "%s: unaligned buffer", __func__);
+	WARN((long)buf & 3, "%s: unaligned buffer", __func__);
 	wdev->hwbus_ops->lock(wdev->hwbus_priv);
 	ret = wdev->hwbus_ops->copy_to_io(wdev->hwbus_priv,
 					  WFX_REG_IN_OUT_QUEUE, buf, len);
diff --git a/drivers/staging/wfx/key.c b/drivers/staging/wfx/key.c
index 96adfa330604..5ee2ffc5f935 100644
--- a/drivers/staging/wfx/key.c
+++ b/drivers/staging/wfx/key.c
@@ -5,6 +5,7 @@
  * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
  * Copyright (c) 2010, ST-Ericsson
  */
+#include <linux/etherdevice.h>
 #include <net/mac80211.h>
 
 #include "key.h"
@@ -20,14 +21,12 @@ static int wfx_alloc_key(struct wfx_dev *wdev)
 		return -1;
 
 	wdev->key_map |= BIT(idx);
-	wdev->keys[idx].entry_index = idx;
 	return idx;
 }
 
 static void wfx_free_key(struct wfx_dev *wdev, int idx)
 {
 	WARN(!(wdev->key_map & BIT(idx)), "inconsistent key allocation");
-	memset(&wdev->keys[idx], 0, sizeof(wdev->keys[idx]));
 	wdev->key_map &= ~BIT(idx);
 }
 
@@ -159,7 +158,7 @@ static int wfx_add_key(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 		       struct ieee80211_key_conf *key)
 {
 	int ret;
-	struct hif_req_add_key *k;
+	struct hif_req_add_key k = { };
 	struct ieee80211_key_seq seq;
 	struct wfx_dev *wdev = wvif->wdev;
 	int idx = wfx_alloc_key(wvif->wdev);
@@ -169,44 +168,44 @@ static int wfx_add_key(struct wfx_vif *wvif, struct ieee80211_sta *sta,
 	ieee80211_get_key_rx_seq(key, 0, &seq);
 	if (idx < 0)
 		return -EINVAL;
-	k = &wdev->keys[idx];
-	k->int_id = wvif->id;
+	k.int_id = wvif->id;
+	k.entry_index = idx;
 	if (key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
 	    key->cipher ==  WLAN_CIPHER_SUITE_WEP104) {
 		if (pairwise)
-			k->type = fill_wep_pair(&k->key.wep_pairwise_key, key,
-						sta->addr);
+			k.type = fill_wep_pair(&k.key.wep_pairwise_key, key,
+					       sta->addr);
 		else
-			k->type = fill_wep_group(&k->key.wep_group_key, key);
+			k.type = fill_wep_group(&k.key.wep_group_key, key);
 	} else if (key->cipher == WLAN_CIPHER_SUITE_TKIP) {
 		if (pairwise)
-			k->type = fill_tkip_pair(&k->key.tkip_pairwise_key, key,
-						 sta->addr);
+			k.type = fill_tkip_pair(&k.key.tkip_pairwise_key, key,
+						sta->addr);
 		else
-			k->type = fill_tkip_group(&k->key.tkip_group_key, key,
-						  &seq, wvif->vif->type);
+			k.type = fill_tkip_group(&k.key.tkip_group_key, key,
+						 &seq, wvif->vif->type);
 	} else if (key->cipher == WLAN_CIPHER_SUITE_CCMP) {
 		if (pairwise)
-			k->type = fill_ccmp_pair(&k->key.aes_pairwise_key, key,
-						 sta->addr);
+			k.type = fill_ccmp_pair(&k.key.aes_pairwise_key, key,
+						sta->addr);
 		else
-			k->type = fill_ccmp_group(&k->key.aes_group_key, key,
-						  &seq);
+			k.type = fill_ccmp_group(&k.key.aes_group_key, key,
+						 &seq);
 	} else if (key->cipher ==  WLAN_CIPHER_SUITE_SMS4) {
 		if (pairwise)
-			k->type = fill_sms4_pair(&k->key.wapi_pairwise_key, key,
-						 sta->addr);
+			k.type = fill_sms4_pair(&k.key.wapi_pairwise_key, key,
+						sta->addr);
 		else
-			k->type = fill_sms4_group(&k->key.wapi_group_key, key);
+			k.type = fill_sms4_group(&k.key.wapi_group_key, key);
 	} else if (key->cipher ==  WLAN_CIPHER_SUITE_AES_CMAC) {
-		k->type = fill_aes_cmac_group(&k->key.igtk_group_key, key,
-					      &seq);
+		k.type = fill_aes_cmac_group(&k.key.igtk_group_key, key,
+					     &seq);
 	} else {
 		dev_warn(wdev->dev, "unsupported key type %d\n", key->cipher);
 		wfx_free_key(wdev, idx);
 		return -EOPNOTSUPP;
 	}
-	ret = hif_add_key(wdev, k);
+	ret = hif_add_key(wdev, &k);
 	if (ret) {
 		wfx_free_key(wdev, idx);
 		return -EOPNOTSUPP;
@@ -229,7 +228,7 @@ int wfx_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		struct ieee80211_key_conf *key)
 {
 	int ret = -EOPNOTSUPP;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	mutex_lock(&wvif->wdev->conf_mutex);
 	if (cmd == SET_KEY)
@@ -240,29 +239,3 @@ int wfx_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return ret;
 }
 
-int wfx_upload_keys(struct wfx_vif *wvif)
-{
-	int i;
-	struct hif_req_add_key *key;
-	struct wfx_dev *wdev = wvif->wdev;
-
-	for (i = 0; i < ARRAY_SIZE(wdev->keys); i++) {
-		if (wdev->key_map & BIT(i)) {
-			key = &wdev->keys[i];
-			if (key->int_id == wvif->id)
-				hif_add_key(wdev, key);
-		}
-	}
-	return 0;
-}
-
-void wfx_wep_key_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif, wep_key_work);
-
-	wfx_tx_flush(wvif->wdev);
-	hif_wep_default_key_id(wvif, wvif->wep_default_key_id);
-	wfx_pending_requeue(wvif->wdev, wvif->wep_pending_skb);
-	wvif->wep_pending_skb = NULL;
-	wfx_tx_unlock(wvif->wdev);
-}
diff --git a/drivers/staging/wfx/key.h b/drivers/staging/wfx/key.h
index 9436ccdf4d3b..ff31fc9c565a 100644
--- a/drivers/staging/wfx/key.h
+++ b/drivers/staging/wfx/key.h
@@ -16,7 +16,5 @@ struct wfx_vif;
 int wfx_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		struct ieee80211_vif *vif, struct ieee80211_sta *sta,
 		struct ieee80211_key_conf *key);
-int wfx_upload_keys(struct wfx_vif *wvif);
-void wfx_wep_key_work(struct work_struct *work);
 
 #endif /* WFX_STA_H */
diff --git a/drivers/staging/wfx/main.c b/drivers/staging/wfx/main.c
index 3c4c240229ad..6bd96f476388 100644
--- a/drivers/staging/wfx/main.c
+++ b/drivers/staging/wfx/main.c
@@ -28,6 +28,7 @@
 #include "bh.h"
 #include "sta.h"
 #include "key.h"
+#include "scan.h"
 #include "debug.h"
 #include "data_tx.h"
 #include "secure_link.h"
@@ -106,7 +107,7 @@ static const struct ieee80211_supported_band wfx_band_2ghz = {
 		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
 		.mcs = {
 			.rx_mask = { 0xFF }, // MCS0 to MCS7
-			.rx_highest = 65,
+			.rx_highest = cpu_to_le16(72),
 			.tx_params = IEEE80211_HT_MCS_TX_DEFINED,
 		},
 	},
@@ -133,15 +134,19 @@ static const struct ieee80211_ops wfx_ops = {
 	.remove_interface	= wfx_remove_interface,
 	.config                 = wfx_config,
 	.tx			= wfx_tx,
+	.join_ibss		= wfx_join_ibss,
+	.leave_ibss		= wfx_leave_ibss,
 	.conf_tx		= wfx_conf_tx,
 	.hw_scan		= wfx_hw_scan,
 	.cancel_hw_scan		= wfx_cancel_hw_scan,
+	.start_ap		= wfx_start_ap,
+	.stop_ap		= wfx_stop_ap,
 	.sta_add		= wfx_sta_add,
 	.sta_remove		= wfx_sta_remove,
-	.sta_notify		= wfx_sta_notify,
 	.set_tim		= wfx_set_tim,
 	.set_key		= wfx_set_key,
 	.set_rts_threshold	= wfx_set_rts_threshold,
+	.set_default_unicast_key = wfx_set_default_unicast_key,
 	.bss_info_changed	= wfx_bss_info_changed,
 	.prepare_multicast	= wfx_prepare_multicast,
 	.configure_filter	= wfx_configure_filter,
@@ -165,8 +170,8 @@ bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor)
 	return false;
 }
 
-struct gpio_desc *wfx_get_gpio(struct device *dev, int override,
-			       const char *label)
+struct gpio_desc *wfx_get_gpio(struct device *dev,
+			       int override, const char *label)
 {
 	struct gpio_desc *ret;
 	char label_buf[256];
@@ -187,18 +192,18 @@ struct gpio_desc *wfx_get_gpio(struct device *dev, int override,
 		if (!ret || PTR_ERR(ret) == -ENOENT)
 			dev_warn(dev, "gpio %s is not defined\n", label);
 		else
-			dev_warn(dev,
-				 "error while requesting gpio %s\n", label);
+			dev_warn(dev, "error while requesting gpio %s\n",
+				 label);
 		ret = NULL;
 	} else {
-		dev_dbg(dev,
-			"using gpio %d for %s\n", desc_to_gpio(ret), label);
+		dev_dbg(dev, "using gpio %d for %s\n",
+			desc_to_gpio(ret), label);
 	}
 	return ret;
 }
 
 /* NOTE: wfx_send_pds() destroy buf */
-int wfx_send_pds(struct wfx_dev *wdev, unsigned char *buf, size_t len)
+int wfx_send_pds(struct wfx_dev *wdev, u8 *buf, size_t len)
 {
 	int ret;
 	int start, brace_level, i;
@@ -224,16 +229,19 @@ int wfx_send_pds(struct wfx_dev *wdev, unsigned char *buf, size_t len)
 			buf[i] = '}';
 			ret = hif_configuration(wdev, buf + start,
 						i - start + 1);
-			if (ret == HIF_STATUS_FAILURE) {
-				dev_err(wdev->dev, "PDS bytes %d to %d: invalid data (unsupported options?)\n", start, i);
+			if (ret > 0) {
+				dev_err(wdev->dev, "PDS bytes %d to %d: invalid data (unsupported options?)\n",
+					start, i);
 				return -EINVAL;
 			}
 			if (ret == -ETIMEDOUT) {
-				dev_err(wdev->dev, "PDS bytes %d to %d: chip didn't reply (corrupted file?)\n", start, i);
+				dev_err(wdev->dev, "PDS bytes %d to %d: chip didn't reply (corrupted file?)\n",
+					start, i);
 				return ret;
 			}
 			if (ret) {
-				dev_err(wdev->dev, "PDS bytes %d to %d: chip returned an unknown error\n", start, i);
+				dev_err(wdev->dev, "PDS bytes %d to %d: chip returned an unknown error\n",
+					start, i);
 				return -EIO;
 			}
 			buf[i] = ',';
@@ -247,7 +255,7 @@ static int wfx_send_pdata_pds(struct wfx_dev *wdev)
 {
 	int ret = 0;
 	const struct firmware *pds;
-	unsigned char *tmp_buf;
+	u8 *tmp_buf;
 
 	ret = request_firmware(&pds, wdev->pdata.file_pds, wdev->dev);
 	if (ret) {
@@ -266,9 +274,9 @@ static void wfx_free_common(void *data)
 {
 	struct wfx_dev *wdev = data;
 
+	mutex_destroy(&wdev->tx_power_loop_info_lock);
 	mutex_destroy(&wdev->rx_stats_lock);
 	mutex_destroy(&wdev->conf_mutex);
-	wfx_tx_queues_deinit(wdev);
 	ieee80211_free_hw(wdev->hw);
 }
 
@@ -286,7 +294,6 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 
 	SET_IEEE80211_DEV(hw, dev);
 
-	ieee80211_hw_set(hw, NEED_DTIM_BEFORE_ASSOC);
 	ieee80211_hw_set(hw, TX_AMPDU_SETUP_IN_HW);
 	ieee80211_hw_set(hw, AMPDU_AGGREGATION);
 	ieee80211_hw_set(hw, CONNECTION_MONITOR);
@@ -315,7 +322,7 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 	hw->wiphy->flags |= WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD;
 	hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
 	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
-	hw->wiphy->max_ap_assoc_sta = WFX_MAX_STA_IN_AP_MODE;
+	hw->wiphy->max_ap_assoc_sta = HIF_LINK_ID_MAX;
 	hw->wiphy->max_scan_ssids = 2;
 	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
 	hw->wiphy->n_iface_combinations = ARRAY_SIZE(wfx_iface_combinations);
@@ -338,7 +345,10 @@ struct wfx_dev *wfx_init_common(struct device *dev,
 
 	mutex_init(&wdev->conf_mutex);
 	mutex_init(&wdev->rx_stats_lock);
+	mutex_init(&wdev->tx_power_loop_info_lock);
 	init_completion(&wdev->firmware_ready);
+	INIT_DELAYED_WORK(&wdev->cooling_timeout_work,
+			  wfx_cooling_timeout_work);
 	wfx_init_hif_cmd(&wdev->hif_cmd);
 	wfx_tx_queues_init(wdev);
 
@@ -359,23 +369,24 @@ int wfx_probe(struct wfx_dev *wdev)
 	// prevent bh() to touch it.
 	gpio_saved = wdev->pdata.gpio_wakeup;
 	wdev->pdata.gpio_wakeup = NULL;
+	wdev->poll_irq = true;
 
 	wfx_bh_register(wdev);
 
 	err = wfx_init_device(wdev);
 	if (err)
-		goto err1;
+		goto err0;
 
-	err = wait_for_completion_interruptible_timeout(&wdev->firmware_ready,
-							10 * HZ);
+	wfx_bh_poll_irq(wdev);
+	err = wait_for_completion_timeout(&wdev->firmware_ready, 1 * HZ);
 	if (err <= 0) {
 		if (err == 0) {
-			dev_err(wdev->dev, "timeout while waiting for startup indication. IRQ configuration error?\n");
+			dev_err(wdev->dev, "timeout while waiting for startup indication\n");
 			err = -ETIMEDOUT;
 		} else if (err == -ERESTARTSYS) {
 			dev_info(wdev->dev, "probe interrupted by user\n");
 		}
-		goto err1;
+		goto err0;
 	}
 
 	// FIXME: fill wiphy::hw_version
@@ -384,7 +395,7 @@ int wfx_probe(struct wfx_dev *wdev)
 		 wdev->hw_caps.firmware_build, wdev->hw_caps.firmware_label,
 		 wdev->hw_caps.api_version_major,
 		 wdev->hw_caps.api_version_minor,
-		 wdev->keyset, *((u32 *) &wdev->hw_caps.capabilities));
+		 wdev->keyset, *((u32 *)&wdev->hw_caps.capabilities));
 	snprintf(wdev->hw->wiphy->fw_version,
 		 sizeof(wdev->hw->wiphy->fw_version),
 		 "%d.%d.%d",
@@ -397,14 +408,14 @@ int wfx_probe(struct wfx_dev *wdev)
 			"unsupported firmware API version (expect 1 while firmware returns %d)\n",
 			wdev->hw_caps.api_version_major);
 		err = -ENOTSUPP;
-		goto err1;
+		goto err0;
 	}
 
 	err = wfx_sl_init(wdev);
 	if (err && wdev->hw_caps.capabilities.link_mode == SEC_LINK_ENFORCED) {
 		dev_err(wdev->dev,
 			"chip require secure_link, but can't negociate it\n");
-		goto err1;
+		goto err0;
 	}
 
 	if (wdev->hw_caps.regul_sel_mode_info.region_sel_mode) {
@@ -417,7 +428,16 @@ int wfx_probe(struct wfx_dev *wdev)
 		wdev->pdata.file_pds);
 	err = wfx_send_pdata_pds(wdev);
 	if (err < 0)
-		goto err1;
+		goto err0;
+
+	wdev->poll_irq = false;
+	err = wdev->hwbus_ops->irq_subscribe(wdev->hwbus_priv);
+	if (err)
+		goto err0;
+
+	err = hif_use_multi_tx_conf(wdev, true);
+	if (err)
+		dev_err(wdev->dev, "misconfigured IRQ?\n");
 
 	wdev->pdata.gpio_wakeup = gpio_saved;
 	if (wdev->pdata.gpio_wakeup) {
@@ -432,8 +452,6 @@ int wfx_probe(struct wfx_dev *wdev)
 		hif_set_operational_mode(wdev, HIF_OP_POWER_MODE_DOZE);
 	}
 
-	hif_use_multi_tx_conf(wdev, true);
-
 	for (i = 0; i < ARRAY_SIZE(wdev->addresses); i++) {
 		eth_zero_addr(wdev->addresses[i].addr);
 		macaddr = of_get_mac_address(wdev->dev->of_node);
@@ -466,8 +484,9 @@ int wfx_probe(struct wfx_dev *wdev)
 
 err2:
 	ieee80211_unregister_hw(wdev->hw);
-	ieee80211_free_hw(wdev->hw);
 err1:
+	wdev->hwbus_ops->irq_unsubscribe(wdev->hwbus_priv);
+err0:
 	wfx_bh_unregister(wdev);
 	return err;
 }
@@ -476,6 +495,7 @@ void wfx_release(struct wfx_dev *wdev)
 {
 	ieee80211_unregister_hw(wdev->hw);
 	hif_shutdown(wdev);
+	wdev->hwbus_ops->irq_unsubscribe(wdev->hwbus_priv);
 	wfx_bh_unregister(wdev);
 	wfx_sl_deinit(wdev);
 }
diff --git a/drivers/staging/wfx/main.h b/drivers/staging/wfx/main.h
index 9c9410072def..f832ce409fda 100644
--- a/drivers/staging/wfx/main.h
+++ b/drivers/staging/wfx/main.h
@@ -13,10 +13,10 @@
 #include <linux/device.h>
 #include <linux/gpio/consumer.h>
 
-#include "bus.h"
 #include "hif_api_general.h"
 
 struct wfx_dev;
+struct hwbus_ops;
 
 struct wfx_platform_data {
 	/* Keyset and ".sec" extention will appended to this string */
@@ -41,6 +41,6 @@ void wfx_release(struct wfx_dev *wdev);
 struct gpio_desc *wfx_get_gpio(struct device *dev, int override,
 			       const char *label);
 bool wfx_api_older_than(struct wfx_dev *wdev, int major, int minor);
-int wfx_send_pds(struct wfx_dev *wdev, unsigned char *buf, size_t len);
+int wfx_send_pds(struct wfx_dev *wdev, u8 *buf, size_t len);
 
 #endif
diff --git a/drivers/staging/wfx/queue.c b/drivers/staging/wfx/queue.c
index 39d9127ce4b9..3248ecefda56 100644
--- a/drivers/staging/wfx/queue.c
+++ b/drivers/staging/wfx/queue.c
@@ -35,6 +35,7 @@ void wfx_tx_flush(struct wfx_dev *wdev)
 	if (wdev->chip_frozen)
 		return;
 
+	wfx_tx_lock(wdev);
 	mutex_lock(&wdev->hif_cmd.lock);
 	ret = wait_event_timeout(wdev->hif.tx_buffers_empty,
 				 !wdev->hif.tx_buffers_used,
@@ -44,9 +45,10 @@ void wfx_tx_flush(struct wfx_dev *wdev)
 			 wdev->hif.tx_buffers_used);
 		wfx_pending_dump_old_frames(wdev, 3000);
 		// FIXME: drop pending frames here
-		wdev->chip_frozen = 1;
+		wdev->chip_frozen = true;
 	}
 	mutex_unlock(&wdev->hif_cmd.lock);
+	wfx_tx_unlock(wdev);
 }
 
 void wfx_tx_lock_flush(struct wfx_dev *wdev)
@@ -55,261 +57,142 @@ void wfx_tx_lock_flush(struct wfx_dev *wdev)
 	wfx_tx_flush(wdev);
 }
 
-void wfx_tx_queues_lock(struct wfx_dev *wdev)
+void wfx_tx_queues_init(struct wfx_dev *wdev)
 {
 	int i;
-	struct wfx_queue *queue;
 
+	skb_queue_head_init(&wdev->tx_pending);
+	init_waitqueue_head(&wdev->tx_dequeue);
 	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		queue = &wdev->tx_queue[i];
-		spin_lock_bh(&queue->queue.lock);
-		if (queue->tx_locked_cnt++ == 0)
-			ieee80211_stop_queue(wdev->hw, queue->queue_id);
-		spin_unlock_bh(&queue->queue.lock);
+		skb_queue_head_init(&wdev->tx_queue[i].normal);
+		skb_queue_head_init(&wdev->tx_queue[i].cab);
 	}
 }
 
-void wfx_tx_queues_unlock(struct wfx_dev *wdev)
+void wfx_tx_queues_check_empty(struct wfx_dev *wdev)
 {
 	int i;
-	struct wfx_queue *queue;
 
+	WARN_ON(!skb_queue_empty_lockless(&wdev->tx_pending));
 	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		queue = &wdev->tx_queue[i];
-		spin_lock_bh(&queue->queue.lock);
-		WARN(!queue->tx_locked_cnt, "queue already unlocked");
-		if (--queue->tx_locked_cnt == 0)
-			ieee80211_wake_queue(wdev->hw, queue->queue_id);
-		spin_unlock_bh(&queue->queue.lock);
+		WARN_ON(atomic_read(&wdev->tx_queue[i].pending_frames));
+		WARN_ON(!skb_queue_empty_lockless(&wdev->tx_queue[i].normal));
+		WARN_ON(!skb_queue_empty_lockless(&wdev->tx_queue[i].cab));
 	}
 }
 
-/* If successful, LOCKS the TX queue! */
-void wfx_tx_queues_wait_empty_vif(struct wfx_vif *wvif)
+static bool __wfx_tx_queue_empty(struct wfx_dev *wdev,
+				 struct sk_buff_head *skb_queue, int vif_id)
 {
-	int i;
-	bool done;
-	struct wfx_queue *queue;
-	struct sk_buff *item;
-	struct wfx_dev *wdev = wvif->wdev;
-	struct hif_msg *hif;
-
-	if (wvif->wdev->chip_frozen) {
-		wfx_tx_lock_flush(wdev);
-		wfx_tx_queues_clear(wdev);
-		return;
-	}
+	struct hif_msg *hif_msg;
+	struct sk_buff *skb;
 
-	do {
-		done = true;
-		wfx_tx_lock_flush(wdev);
-		for (i = 0; i < IEEE80211_NUM_ACS && done; ++i) {
-			queue = &wdev->tx_queue[i];
-			spin_lock_bh(&queue->queue.lock);
-			skb_queue_walk(&queue->queue, item) {
-				hif = (struct hif_msg *) item->data;
-				if (hif->interface == wvif->id)
-					done = false;
-			}
-			spin_unlock_bh(&queue->queue.lock);
-		}
-		if (!done) {
-			wfx_tx_unlock(wdev);
-			msleep(20);
+	spin_lock_bh(&skb_queue->lock);
+	skb_queue_walk(skb_queue, skb) {
+		hif_msg = (struct hif_msg *)skb->data;
+		if (vif_id < 0 || hif_msg->interface == vif_id) {
+			spin_unlock_bh(&skb_queue->lock);
+			return false;
 		}
-	} while (!done);
-}
-
-static void wfx_tx_queue_clear(struct wfx_dev *wdev, struct wfx_queue *queue,
-			       struct sk_buff_head *gc_list)
-{
-	int i;
-	struct sk_buff *item;
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-
-	spin_lock_bh(&queue->queue.lock);
-	while ((item = __skb_dequeue(&queue->queue)) != NULL)
-		skb_queue_head(gc_list, item);
-	spin_lock_nested(&stats->pending.lock, 1);
-	for (i = 0; i < ARRAY_SIZE(stats->link_map_cache); ++i) {
-		stats->link_map_cache[i] -= queue->link_map_cache[i];
-		queue->link_map_cache[i] = 0;
-	}
-	spin_unlock(&stats->pending.lock);
-	spin_unlock_bh(&queue->queue.lock);
-}
-
-void wfx_tx_queues_clear(struct wfx_dev *wdev)
-{
-	int i;
-	struct sk_buff *item;
-	struct sk_buff_head gc_list;
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-
-	skb_queue_head_init(&gc_list);
-	for (i = 0; i < IEEE80211_NUM_ACS; ++i)
-		wfx_tx_queue_clear(wdev, &wdev->tx_queue[i], &gc_list);
-	wake_up(&stats->wait_link_id_empty);
-	while ((item = skb_dequeue(&gc_list)) != NULL)
-		wfx_skb_dtor(wdev, item);
-}
-
-void wfx_tx_queues_init(struct wfx_dev *wdev)
-{
-	int i;
-
-	memset(&wdev->tx_queue_stats, 0, sizeof(wdev->tx_queue_stats));
-	memset(wdev->tx_queue, 0, sizeof(wdev->tx_queue));
-	skb_queue_head_init(&wdev->tx_queue_stats.pending);
-	init_waitqueue_head(&wdev->tx_queue_stats.wait_link_id_empty);
-
-	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		wdev->tx_queue[i].queue_id = i;
-		skb_queue_head_init(&wdev->tx_queue[i].queue);
 	}
+	spin_unlock_bh(&skb_queue->lock);
+	return true;
 }
 
-void wfx_tx_queues_deinit(struct wfx_dev *wdev)
+bool wfx_tx_queue_empty(struct wfx_dev *wdev,
+			struct wfx_queue *queue, int vif_id)
 {
-	WARN_ON(!skb_queue_empty(&wdev->tx_queue_stats.pending));
-	wfx_tx_queues_clear(wdev);
+	return __wfx_tx_queue_empty(wdev, &queue->normal, vif_id) &&
+	       __wfx_tx_queue_empty(wdev, &queue->cab, vif_id);
 }
 
-int wfx_tx_queue_get_num_queued(struct wfx_queue *queue, u32 link_id_map)
+static void __wfx_tx_queue_drop(struct wfx_dev *wdev,
+				struct sk_buff_head *skb_queue, int vif_id,
+				struct sk_buff_head *dropped)
 {
-	int ret, i;
-
-	if (!link_id_map)
-		return 0;
-
-	spin_lock_bh(&queue->queue.lock);
-	if (link_id_map == (u32)-1) {
-		ret = skb_queue_len(&queue->queue);
-	} else {
-		ret = 0;
-		for (i = 0; i < ARRAY_SIZE(queue->link_map_cache); i++)
-			if (link_id_map & BIT(i))
-				ret += queue->link_map_cache[i];
+	struct sk_buff *skb, *tmp;
+	struct hif_msg *hif_msg;
+
+	spin_lock_bh(&skb_queue->lock);
+	skb_queue_walk_safe(skb_queue, skb, tmp) {
+		hif_msg = (struct hif_msg *)skb->data;
+		if (vif_id < 0 || hif_msg->interface == vif_id) {
+			__skb_unlink(skb, skb_queue);
+			skb_queue_head(dropped, skb);
+		}
 	}
-	spin_unlock_bh(&queue->queue.lock);
-	return ret;
+	spin_unlock_bh(&skb_queue->lock);
 }
 
-void wfx_tx_queue_put(struct wfx_dev *wdev, struct wfx_queue *queue,
-		      struct sk_buff *skb)
+void wfx_tx_queue_drop(struct wfx_dev *wdev, struct wfx_queue *queue,
+		       int vif_id, struct sk_buff_head *dropped)
 {
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-	struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
-
-	WARN(tx_priv->link_id >= ARRAY_SIZE(stats->link_map_cache), "invalid link-id value");
-	spin_lock_bh(&queue->queue.lock);
-	__skb_queue_tail(&queue->queue, skb);
-
-	++queue->link_map_cache[tx_priv->link_id];
-
-	spin_lock_nested(&stats->pending.lock, 1);
-	++stats->link_map_cache[tx_priv->link_id];
-	spin_unlock(&stats->pending.lock);
-	spin_unlock_bh(&queue->queue.lock);
-}
-
-static struct sk_buff *wfx_tx_queue_get(struct wfx_dev *wdev,
-					struct wfx_queue *queue,
-					u32 link_id_map)
-{
-	struct sk_buff *skb = NULL;
-	struct sk_buff *item;
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-	struct wfx_tx_priv *tx_priv;
-	bool wakeup_stats = false;
-
-	spin_lock_bh(&queue->queue.lock);
-	skb_queue_walk(&queue->queue, item) {
-		tx_priv = wfx_skb_tx_priv(item);
-		if (link_id_map & BIT(tx_priv->link_id)) {
-			skb = item;
-			break;
-		}
-	}
-	if (skb) {
-		tx_priv = wfx_skb_tx_priv(skb);
-		tx_priv->xmit_timestamp = ktime_get();
-		__skb_unlink(skb, &queue->queue);
-		--queue->link_map_cache[tx_priv->link_id];
-
-		spin_lock_nested(&stats->pending.lock, 1);
-		__skb_queue_tail(&stats->pending, skb);
-		if (!--stats->link_map_cache[tx_priv->link_id])
-			wakeup_stats = true;
-		spin_unlock(&stats->pending.lock);
-	}
-	spin_unlock_bh(&queue->queue.lock);
-	if (wakeup_stats)
-		wake_up(&stats->wait_link_id_empty);
-	return skb;
+	__wfx_tx_queue_drop(wdev, &queue->cab, vif_id, dropped);
+	__wfx_tx_queue_drop(wdev, &queue->normal, vif_id, dropped);
+	wake_up(&wdev->tx_dequeue);
 }
 
-int wfx_pending_requeue(struct wfx_dev *wdev, struct sk_buff *skb)
+void wfx_tx_queues_put(struct wfx_dev *wdev, struct sk_buff *skb)
 {
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-	struct wfx_tx_priv *tx_priv = wfx_skb_tx_priv(skb);
 	struct wfx_queue *queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 
-	WARN_ON(skb_get_queue_mapping(skb) > 3);
-	spin_lock_bh(&queue->queue.lock);
-	++queue->link_map_cache[tx_priv->link_id];
-
-	spin_lock_nested(&stats->pending.lock, 1);
-	++stats->link_map_cache[tx_priv->link_id];
-	__skb_unlink(skb, &stats->pending);
-	spin_unlock(&stats->pending.lock);
-	__skb_queue_tail(&queue->queue, skb);
-	spin_unlock_bh(&queue->queue.lock);
-	return 0;
+	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM)
+		skb_queue_tail(&queue->cab, skb);
+	else
+		skb_queue_tail(&queue->normal, skb);
 }
 
-int wfx_pending_remove(struct wfx_dev *wdev, struct sk_buff *skb)
+void wfx_pending_drop(struct wfx_dev *wdev, struct sk_buff_head *dropped)
 {
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
-
-	spin_lock_bh(&stats->pending.lock);
-	__skb_unlink(skb, &stats->pending);
-	spin_unlock_bh(&stats->pending.lock);
-	wfx_skb_dtor(wdev, skb);
+	struct wfx_queue *queue;
+	struct sk_buff *skb;
 
-	return 0;
+	WARN(!wdev->chip_frozen, "%s should only be used to recover a frozen device",
+	     __func__);
+	while ((skb = skb_dequeue(&wdev->tx_pending)) != NULL) {
+		queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
+		WARN_ON(skb_get_queue_mapping(skb) > 3);
+		WARN_ON(!atomic_read(&queue->pending_frames));
+		atomic_dec(&queue->pending_frames);
+		skb_queue_head(dropped, skb);
+	}
 }
 
 struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id)
 {
-	struct sk_buff *skb;
+	struct wfx_queue *queue;
 	struct hif_req_tx *req;
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
+	struct sk_buff *skb;
 
-	spin_lock_bh(&stats->pending.lock);
-	skb_queue_walk(&stats->pending, skb) {
+	spin_lock_bh(&wdev->tx_pending.lock);
+	skb_queue_walk(&wdev->tx_pending, skb) {
 		req = wfx_skb_txreq(skb);
 		if (req->packet_id == packet_id) {
-			spin_unlock_bh(&stats->pending.lock);
+			spin_unlock_bh(&wdev->tx_pending.lock);
+			queue = &wdev->tx_queue[skb_get_queue_mapping(skb)];
+			WARN_ON(skb_get_queue_mapping(skb) > 3);
+			WARN_ON(!atomic_read(&queue->pending_frames));
+			atomic_dec(&queue->pending_frames);
+			skb_unlink(skb, &wdev->tx_pending);
 			return skb;
 		}
 	}
-	spin_unlock_bh(&stats->pending.lock);
+	spin_unlock_bh(&wdev->tx_pending.lock);
 	WARN(1, "cannot find packet in pending queue");
 	return NULL;
 }
 
 void wfx_pending_dump_old_frames(struct wfx_dev *wdev, unsigned int limit_ms)
 {
-	struct wfx_queue_stats *stats = &wdev->tx_queue_stats;
 	ktime_t now = ktime_get();
 	struct wfx_tx_priv *tx_priv;
 	struct hif_req_tx *req;
 	struct sk_buff *skb;
 	bool first = true;
 
-	spin_lock_bh(&stats->pending.lock);
-	skb_queue_walk(&stats->pending, skb) {
+	spin_lock_bh(&wdev->tx_pending.lock);
+	skb_queue_walk(&wdev->tx_pending, skb) {
 		tx_priv = wfx_skb_tx_priv(skb);
 		req = wfx_skb_txreq(skb);
 		if (ktime_after(now, ktime_add_ms(tx_priv->xmit_timestamp,
@@ -324,7 +207,7 @@ void wfx_pending_dump_old_frames(struct wfx_dev *wdev, unsigned int limit_ms)
 				 ktime_ms_delta(now, tx_priv->xmit_timestamp));
 		}
 	}
-	spin_unlock_bh(&stats->pending.lock);
+	spin_unlock_bh(&wdev->tx_pending.lock);
 }
 
 unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
@@ -336,138 +219,66 @@ unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
 	return ktime_us_delta(now, tx_priv->xmit_timestamp);
 }
 
-bool wfx_tx_queues_is_empty(struct wfx_dev *wdev)
+bool wfx_tx_queues_has_cab(struct wfx_vif *wvif)
 {
+	struct wfx_dev *wdev = wvif->wdev;
 	int i;
-	struct sk_buff_head *queue;
-	bool ret = true;
-
-	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
-		queue = &wdev->tx_queue[i].queue;
-		spin_lock_bh(&queue->lock);
-		if (!skb_queue_empty(queue))
-			ret = false;
-		spin_unlock_bh(&queue->lock);
-	}
-	return ret;
-}
-
-static bool hif_handle_tx_data(struct wfx_vif *wvif, struct sk_buff *skb,
-			       struct wfx_queue *queue)
-{
-	struct hif_req_tx *req = wfx_skb_txreq(skb);
-	struct ieee80211_key_conf *hw_key = wfx_skb_tx_priv(skb)->hw_key;
-	struct ieee80211_hdr *frame =
-		(struct ieee80211_hdr *)(req->frame + req->data_flags.fc_offset);
-
-	// FIXME: mac80211 is smart enough to handle BSS loss. Driver should not
-	// try to do anything about that.
-	if (ieee80211_is_nullfunc(frame->frame_control)) {
-		mutex_lock(&wvif->bss_loss_lock);
-		if (wvif->bss_loss_state) {
-			wvif->bss_loss_confirm_id = req->packet_id;
-			req->queue_id.queue_id = HIF_QUEUE_ID_VOICE;
-		}
-		mutex_unlock(&wvif->bss_loss_lock);
-	}
 
-	// FIXME: identify the exact scenario matched by this condition. Does it
-	// happen yet?
-	if (ieee80211_has_protected(frame->frame_control) &&
-	    hw_key && hw_key->keyidx != wvif->wep_default_key_id &&
-	    (hw_key->cipher == WLAN_CIPHER_SUITE_WEP40 ||
-	     hw_key->cipher == WLAN_CIPHER_SUITE_WEP104)) {
-		wfx_tx_lock(wvif->wdev);
-		WARN_ON(wvif->wep_pending_skb);
-		wvif->wep_default_key_id = hw_key->keyidx;
-		wvif->wep_pending_skb = skb;
-		if (!schedule_work(&wvif->wep_key_work))
-			wfx_tx_unlock(wvif->wdev);
-		return true;
-	} else {
+	if (wvif->vif->type != NL80211_IFTYPE_AP)
 		return false;
-	}
-}
-
-static int wfx_get_prio_queue(struct wfx_vif *wvif,
-				 u32 tx_allowed_mask, int *total)
-{
-	static const int urgent = BIT(WFX_LINK_ID_AFTER_DTIM) |
-		BIT(WFX_LINK_ID_UAPSD);
-	const struct ieee80211_tx_queue_params *edca;
-	unsigned int score, best = -1;
-	int winner = -1;
-	int i;
-
-	/* search for a winner using edca params */
-	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		int queued;
-
-		edca = &wvif->edca_params[i];
-		queued = wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[i],
-				tx_allowed_mask);
-		if (!queued)
-			continue;
-		*total += queued;
-		score = ((edca->aifs + edca->cw_min) << 16) +
-			((edca->cw_max - edca->cw_min) *
-			 (get_random_int() & 0xFFFF));
-		if (score < best && (winner < 0 || i != 3)) {
-			best = score;
-			winner = i;
-		}
-	}
-
-	/* override winner if bursting */
-	if (winner >= 0 && wvif->wdev->tx_burst_idx >= 0 &&
-	    winner != wvif->wdev->tx_burst_idx &&
-	    !wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[winner],
-					 tx_allowed_mask & urgent) &&
-	    wfx_tx_queue_get_num_queued(&wvif->wdev->tx_queue[wvif->wdev->tx_burst_idx], tx_allowed_mask))
-		winner = wvif->wdev->tx_burst_idx;
-
-	return winner;
-}
-
-static int wfx_tx_queue_mask_get(struct wfx_vif *wvif,
-				     struct wfx_queue **queue_p,
-				     u32 *tx_allowed_mask_p)
-{
-	int idx;
-	u32 tx_allowed_mask;
-	int total = 0;
-
-	/* Search for unicast traffic */
-	tx_allowed_mask = ~wvif->sta_asleep_mask;
-	tx_allowed_mask |= BIT(WFX_LINK_ID_UAPSD);
-	if (wvif->sta_asleep_mask)
-		tx_allowed_mask &= ~BIT(WFX_LINK_ID_AFTER_DTIM);
-	else
-		tx_allowed_mask |= BIT(WFX_LINK_ID_AFTER_DTIM);
-	idx = wfx_get_prio_queue(wvif, tx_allowed_mask, &total);
-	if (idx < 0)
-		return -ENOENT;
-
-	*queue_p = &wvif->wdev->tx_queue[idx];
-	*tx_allowed_mask_p = tx_allowed_mask;
-	return 0;
+	for (i = 0; i < IEEE80211_NUM_ACS; ++i)
+		// Note: since only AP can have mcast frames in queue and only
+		// one vif can be AP, all queued frames has same interface id
+		if (!skb_queue_empty_lockless(&wdev->tx_queue[i].cab))
+			return true;
+	return false;
 }
 
-struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif)
+static struct sk_buff *wfx_tx_queues_get_skb(struct wfx_dev *wdev)
 {
-	struct wfx_dev *wdev = wvif->wdev;
-	struct ieee80211_tx_info *tx_info;
+	struct wfx_queue *sorted_queues[IEEE80211_NUM_ACS];
+	struct wfx_vif *wvif;
 	struct hif_msg *hif;
 	struct sk_buff *skb;
-	int i;
+	int i, j;
 
-	for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-		skb_queue_walk(&wdev->tx_queue[i].queue, skb) {
-			tx_info = IEEE80211_SKB_CB(skb);
+	// bubble sort
+	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+		sorted_queues[i] = &wdev->tx_queue[i];
+		for (j = i; j > 0; j--)
+			if (atomic_read(&sorted_queues[j]->pending_frames) >
+			    atomic_read(&sorted_queues[j - 1]->pending_frames))
+				swap(sorted_queues[j - 1], sorted_queues[j]);
+	}
+	wvif = NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
+		if (!wvif->after_dtim_tx_allowed)
+			continue;
+		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+			skb = skb_dequeue(&sorted_queues[i]->cab);
+			if (!skb)
+				continue;
+			// Note: since only AP can have mcast frames in queue
+			// and only one vif can be AP, all queued frames has
+			// same interface id
 			hif = (struct hif_msg *)skb->data;
-			if ((tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
-			    (hif->interface == wvif->id))
-				return (struct hif_msg *)skb->data;
+			WARN_ON(hif->interface != wvif->id);
+			WARN_ON(sorted_queues[i] !=
+				&wdev->tx_queue[skb_get_queue_mapping(skb)]);
+			atomic_inc(&sorted_queues[i]->pending_frames);
+			return skb;
+		}
+		// No more multicast to sent
+		wvif->after_dtim_tx_allowed = false;
+		schedule_work(&wvif->update_tim_work);
+	}
+	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
+		skb = skb_dequeue(&sorted_queues[i]->normal);
+		if (skb) {
+			WARN_ON(sorted_queues[i] !=
+				&wdev->tx_queue[skb_get_queue_mapping(skb)]);
+			atomic_inc(&sorted_queues[i]->pending_frames);
+			return skb;
 		}
 	}
 	return NULL;
@@ -475,90 +286,20 @@ struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif)
 
 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev)
 {
+	struct wfx_tx_priv *tx_priv;
 	struct sk_buff *skb;
-	struct hif_msg *hif = NULL;
-	struct wfx_queue *queue = NULL;
-	struct wfx_queue *vif_queue = NULL;
-	u32 tx_allowed_mask = 0;
-	u32 vif_tx_allowed_mask = 0;
-	struct wfx_vif *wvif;
-	int not_found;
-	int burst;
-	int i;
 
 	if (atomic_read(&wdev->tx_lock))
 		return NULL;
 
-	wvif = NULL;
-	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
-		if (wvif->after_dtim_tx_allowed) {
-			for (i = 0; i < IEEE80211_NUM_ACS; ++i) {
-				skb = wfx_tx_queue_get(wvif->wdev,
-						       &wdev->tx_queue[i],
-						       BIT(WFX_LINK_ID_AFTER_DTIM));
-				if (skb) {
-					hif = (struct hif_msg *)skb->data;
-					// Cannot happen since only one vif can
-					// be AP at time
-					WARN_ON(wvif->id != hif->interface);
-					return hif;
-				}
-			}
-			// No more multicast to sent
-			wvif->after_dtim_tx_allowed = false;
-			schedule_work(&wvif->update_tim_work);
-		}
-	}
-
 	for (;;) {
-		int ret = -ENOENT;
-		int queue_num;
-
-		wvif = NULL;
-		while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
-			spin_lock_bh(&wvif->ps_state_lock);
-
-			not_found = wfx_tx_queue_mask_get(wvif, &vif_queue,
-							  &vif_tx_allowed_mask);
-
-			spin_unlock_bh(&wvif->ps_state_lock);
-
-			if (!not_found) {
-				if (queue && queue != vif_queue)
-					dev_info(wdev->dev, "vifs disagree about queue priority\n");
-				tx_allowed_mask |= vif_tx_allowed_mask;
-				queue = vif_queue;
-				ret = 0;
-			}
-		}
-
-		if (ret)
-			return NULL;
-
-		queue_num = queue - wdev->tx_queue;
-
-		skb = wfx_tx_queue_get(wdev, queue, tx_allowed_mask);
+		skb = wfx_tx_queues_get_skb(wdev);
 		if (!skb)
-			continue;
-		hif = (struct hif_msg *)skb->data;
-		wvif = wdev_to_wvif(wdev, hif->interface);
-		WARN_ON(!wvif);
-
-		if (hif_handle_tx_data(wvif, skb, queue))
-			continue;  /* Handled by WSM */
-
-		/* allow bursting if txop is set */
-		if (wvif->edca_params[queue_num].txop)
-			burst = wfx_tx_queue_get_num_queued(queue, tx_allowed_mask) + 1;
-		else
-			burst = 1;
-
-		/* store index of bursting queue */
-		if (burst > 1)
-			wdev->tx_burst_idx = queue_num;
-		else
-			wdev->tx_burst_idx = -1;
-
-		return hif;
+			return NULL;
+		skb_queue_tail(&wdev->tx_pending, skb);
+		wake_up(&wdev->tx_dequeue);
+		tx_priv = wfx_skb_tx_priv(skb);
+		tx_priv->xmit_timestamp = ktime_get();
+		return (struct hif_msg *)skb->data;
 	}
 }
diff --git a/drivers/staging/wfx/queue.h b/drivers/staging/wfx/queue.h
index 90bb060d1204..0c3b7244498e 100644
--- a/drivers/staging/wfx/queue.h
+++ b/drivers/staging/wfx/queue.h
@@ -9,29 +9,15 @@
 #define WFX_QUEUE_H
 
 #include <linux/skbuff.h>
-
-#include "hif_api_cmd.h"
-
-#define WFX_MAX_STA_IN_AP_MODE    14
-#define WFX_LINK_ID_NO_ASSOC      15
-#define WFX_LINK_ID_AFTER_DTIM    (WFX_LINK_ID_NO_ASSOC + 1)
-#define WFX_LINK_ID_UAPSD         (WFX_LINK_ID_NO_ASSOC + 2)
-#define WFX_LINK_ID_MAX           (WFX_LINK_ID_NO_ASSOC + 3)
+#include <linux/atomic.h>
 
 struct wfx_dev;
 struct wfx_vif;
 
 struct wfx_queue {
-	struct sk_buff_head	queue;
-	int			tx_locked_cnt;
-	int			link_map_cache[WFX_LINK_ID_MAX];
-	u8			queue_id;
-};
-
-struct wfx_queue_stats {
-	int			link_map_cache[WFX_LINK_ID_MAX];
-	struct sk_buff_head	pending;
-	wait_queue_head_t	wait_link_id_empty;
+	struct sk_buff_head	normal;
+	struct sk_buff_head	cab; // Content After (DTIM) Beacon
+	atomic_t		pending_frames;
 };
 
 void wfx_tx_lock(struct wfx_dev *wdev);
@@ -40,22 +26,18 @@ void wfx_tx_flush(struct wfx_dev *wdev);
 void wfx_tx_lock_flush(struct wfx_dev *wdev);
 
 void wfx_tx_queues_init(struct wfx_dev *wdev);
-void wfx_tx_queues_deinit(struct wfx_dev *wdev);
-void wfx_tx_queues_lock(struct wfx_dev *wdev);
-void wfx_tx_queues_unlock(struct wfx_dev *wdev);
-void wfx_tx_queues_clear(struct wfx_dev *wdev);
-bool wfx_tx_queues_is_empty(struct wfx_dev *wdev);
-void wfx_tx_queues_wait_empty_vif(struct wfx_vif *wvif);
+void wfx_tx_queues_check_empty(struct wfx_dev *wdev);
+bool wfx_tx_queues_has_cab(struct wfx_vif *wvif);
+void wfx_tx_queues_put(struct wfx_dev *wdev, struct sk_buff *skb);
 struct hif_msg *wfx_tx_queues_get(struct wfx_dev *wdev);
-struct hif_msg *wfx_tx_queues_get_after_dtim(struct wfx_vif *wvif);
 
-void wfx_tx_queue_put(struct wfx_dev *wdev, struct wfx_queue *queue,
-		      struct sk_buff *skb);
-int wfx_tx_queue_get_num_queued(struct wfx_queue *queue, u32 link_id_map);
+bool wfx_tx_queue_empty(struct wfx_dev *wdev, struct wfx_queue *queue,
+			int vif_id);
+void wfx_tx_queue_drop(struct wfx_dev *wdev, struct wfx_queue *queue,
+		       int vif_id, struct sk_buff_head *dropped);
 
 struct sk_buff *wfx_pending_get(struct wfx_dev *wdev, u32 packet_id);
-int wfx_pending_remove(struct wfx_dev *wdev, struct sk_buff *skb);
-int wfx_pending_requeue(struct wfx_dev *wdev, struct sk_buff *skb);
+void wfx_pending_drop(struct wfx_dev *wdev, struct sk_buff_head *dropped);
 unsigned int wfx_pending_get_pkt_us_delay(struct wfx_dev *wdev,
 					  struct sk_buff *skb);
 void wfx_pending_dump_old_frames(struct wfx_dev *wdev, unsigned int limit_ms);
diff --git a/drivers/staging/wfx/scan.c b/drivers/staging/wfx/scan.c
index 9aa14331affd..57ea9997800b 100644
--- a/drivers/staging/wfx/scan.c
+++ b/drivers/staging/wfx/scan.c
@@ -88,18 +88,22 @@ void wfx_hw_scan_work(struct work_struct *work)
 	struct ieee80211_scan_request *hw_req = wvif->scan_req;
 	int chan_cur, ret;
 
-	mutex_lock(&wvif->scan_lock);
 	mutex_lock(&wvif->wdev->conf_mutex);
+	mutex_lock(&wvif->scan_lock);
+	if (wvif->join_in_progress) {
+		dev_info(wvif->wdev->dev, "%s: abort in-progress REQ_JOIN",
+			 __func__);
+		wfx_reset(wvif);
+	}
 	update_probe_tmpl(wvif, &hw_req->req);
-	wfx_fwd_probe_req(wvif, true);
 	chan_cur = 0;
 	do {
 		ret = send_scan_req(wvif, &hw_req->req, chan_cur);
 		if (ret > 0)
 			chan_cur += ret;
 	} while (ret > 0 && chan_cur < hw_req->req.n_channels);
-	mutex_unlock(&wvif->wdev->conf_mutex);
 	mutex_unlock(&wvif->scan_lock);
+	mutex_unlock(&wvif->wdev->conf_mutex);
 	__ieee80211_scan_completed_compat(wvif->wdev->hw, ret < 0);
 }
 
@@ -113,9 +117,6 @@ int wfx_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	if (vif->type == NL80211_IFTYPE_AP)
 		return -EOPNOTSUPP;
 
-	if (wvif->state == WFX_STATE_PRE_STA)
-		return -EBUSY;
-
 	wvif->scan_req = hw_req;
 	schedule_work(&wvif->scan_work);
 	return 0;
diff --git a/drivers/staging/wfx/sta.c b/drivers/staging/wfx/sta.c
index 9d430346a58b..12e8a5b638f1 100644
--- a/drivers/staging/wfx/sta.c
+++ b/drivers/staging/wfx/sta.c
@@ -5,6 +5,7 @@
  * Copyright (c) 2017-2019, Silicon Laboratories, Inc.
  * Copyright (c) 2010, ST-Ericsson
  */
+#include <linux/etherdevice.h>
 #include <net/mac80211.h>
 
 #include "sta.h"
@@ -37,117 +38,32 @@ u32 wfx_rate_mask_to_hw(struct wfx_dev *wdev, u32 rates)
 	return ret;
 }
 
-static void __wfx_free_event_queue(struct list_head *list)
+void wfx_cooling_timeout_work(struct work_struct *work)
 {
-	struct wfx_hif_event *event, *tmp;
+	struct wfx_dev *wdev = container_of(to_delayed_work(work),
+					    struct wfx_dev,
+					    cooling_timeout_work);
 
-	list_for_each_entry_safe(event, tmp, list, link) {
-		list_del(&event->link);
-		kfree(event);
-	}
-}
-
-static void wfx_free_event_queue(struct wfx_vif *wvif)
-{
-	LIST_HEAD(list);
-
-	spin_lock(&wvif->event_queue_lock);
-	list_splice_init(&wvif->event_queue, &list);
-	spin_unlock(&wvif->event_queue_lock);
-
-	__wfx_free_event_queue(&list);
+	wdev->chip_frozen = true;
+	wfx_tx_unlock(wdev);
 }
 
-void wfx_cqm_bssloss_sm(struct wfx_vif *wvif, int init, int good, int bad)
+void wfx_suspend_hot_dev(struct wfx_dev *wdev, enum sta_notify_cmd cmd)
 {
-	int tx = 0;
-
-	mutex_lock(&wvif->bss_loss_lock);
-	cancel_work_sync(&wvif->bss_params_work);
-
-	if (init) {
-		schedule_delayed_work(&wvif->bss_loss_work, HZ);
-		wvif->bss_loss_state = 0;
-
-		if (!atomic_read(&wvif->wdev->tx_lock))
-			tx = 1;
-	} else if (good) {
-		cancel_delayed_work_sync(&wvif->bss_loss_work);
-		wvif->bss_loss_state = 0;
-		schedule_work(&wvif->bss_params_work);
-	} else if (bad) {
-		/* FIXME Should we just keep going until we time out? */
-		if (wvif->bss_loss_state < 3)
-			tx = 1;
+	if (cmd == STA_NOTIFY_AWAKE) {
+		// Device recover normal temperature
+		if (cancel_delayed_work(&wdev->cooling_timeout_work))
+			wfx_tx_unlock(wdev);
 	} else {
-		cancel_delayed_work_sync(&wvif->bss_loss_work);
-		wvif->bss_loss_state = 0;
-	}
-
-	/* Spit out a NULL packet to our AP if necessary */
-	// FIXME: call ieee80211_beacon_loss/ieee80211_connection_loss instead
-	if (tx) {
-		struct sk_buff *skb;
-		struct ieee80211_hdr *hdr;
-		struct ieee80211_tx_control control = { };
-
-		wvif->bss_loss_state++;
-
-		skb = ieee80211_nullfunc_get(wvif->wdev->hw, wvif->vif, false);
-		if (!skb)
-			goto end;
-		hdr = (struct ieee80211_hdr *)skb->data;
-		memset(IEEE80211_SKB_CB(skb), 0,
-		       sizeof(*IEEE80211_SKB_CB(skb)));
-		IEEE80211_SKB_CB(skb)->control.vif = wvif->vif;
-		IEEE80211_SKB_CB(skb)->driver_rates[0].idx = 0;
-		IEEE80211_SKB_CB(skb)->driver_rates[0].count = 1;
-		IEEE80211_SKB_CB(skb)->driver_rates[1].idx = -1;
-		rcu_read_lock(); // protect control.sta
-		control.sta = ieee80211_find_sta(wvif->vif, hdr->addr1);
-		wfx_tx(wvif->wdev->hw, &control, skb);
-		rcu_read_unlock();
+		// Device is too hot
+		schedule_delayed_work(&wdev->cooling_timeout_work, 10 * HZ);
+		wfx_tx_lock(wdev);
 	}
-end:
-	mutex_unlock(&wvif->bss_loss_lock);
-}
-
-int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable)
-{
-	wvif->fwd_probe_req = enable;
-	return hif_set_rx_filter(wvif, wvif->filter_bssid,
-				 wvif->fwd_probe_req);
-}
-
-static int wfx_set_mcast_filter(struct wfx_vif *wvif,
-				    struct wfx_grp_addr_table *fp)
-{
-	int i;
-
-	// Temporary workaround for filters
-	return hif_set_data_filtering(wvif, false, true);
-
-	if (!fp->enable)
-		return hif_set_data_filtering(wvif, false, true);
-
-	for (i = 0; i < fp->num_addresses; i++)
-		hif_set_mac_addr_condition(wvif, i, fp->address_list[i]);
-	hif_set_uc_mc_bc_condition(wvif, 0,
-				   HIF_FILTER_UNICAST | HIF_FILTER_BROADCAST);
-	hif_set_config_data_filter(wvif, true, 0, BIT(1),
-				   BIT(fp->num_addresses) - 1);
-	hif_set_data_filtering(wvif, true, true);
-
-	return 0;
 }
 
-void wfx_update_filtering(struct wfx_vif *wvif)
+static void wfx_filter_beacon(struct wfx_vif *wvif, bool filter_beacon)
 {
-	int ret;
-	int bf_enable;
-	int bf_count;
-	int n_filter_ies;
-	struct hif_ie_table_entry filter_ies[] = {
+	const struct hif_ie_table_entry filter_ies[] = {
 		{
 			.ie_id        = WLAN_EID_VENDOR_SPECIFIC,
 			.has_changed  = 1,
@@ -167,40 +83,33 @@ void wfx_update_filtering(struct wfx_vif *wvif)
 		}
 	};
 
-	if (wvif->state == WFX_STATE_PASSIVE)
-		return;
-
-	if (wvif->disable_beacon_filter) {
-		bf_enable = 0;
-		bf_count = 1;
-		n_filter_ies = 0;
-	} else if (wvif->vif->type != NL80211_IFTYPE_STATION) {
-		bf_enable = HIF_BEACON_FILTER_ENABLE | HIF_BEACON_FILTER_AUTO_ERP;
-		bf_count = 0;
-		n_filter_ies = 2;
+	if (!filter_beacon) {
+		hif_beacon_filter_control(wvif, 0, 1);
 	} else {
-		bf_enable = HIF_BEACON_FILTER_ENABLE;
-		bf_count = 0;
-		n_filter_ies = 3;
+		hif_set_beacon_filter_table(wvif, 3, filter_ies);
+		hif_beacon_filter_control(wvif, HIF_BEACON_FILTER_ENABLE, 0);
 	}
-
-	ret = hif_set_rx_filter(wvif, wvif->filter_bssid, wvif->fwd_probe_req);
-	if (!ret)
-		ret = hif_set_beacon_filter_table(wvif, n_filter_ies, filter_ies);
-	if (!ret)
-		ret = hif_beacon_filter_control(wvif, bf_enable, bf_count);
-	if (!ret)
-		ret = wfx_set_mcast_filter(wvif, &wvif->mcast_filter);
-	if (ret)
-		dev_err(wvif->wdev->dev, "update filtering failed: %d\n", ret);
 }
 
-static void wfx_update_filtering_work(struct work_struct *work)
+static void wfx_filter_mcast(struct wfx_vif *wvif, bool filter_mcast)
 {
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    update_filtering_work);
+	int i;
 
-	wfx_update_filtering(wvif);
+	// Temporary workaround for filters
+	hif_set_data_filtering(wvif, false, true);
+	return;
+
+	if (!filter_mcast) {
+		hif_set_data_filtering(wvif, false, true);
+		return;
+	}
+	for (i = 0; i < wvif->filter_mcast_count; i++)
+		hif_set_mac_addr_condition(wvif, i, wvif->filter_mcast_addr[i]);
+	hif_set_uc_mc_bc_condition(wvif, 0,
+				   HIF_FILTER_UNICAST | HIF_FILTER_BROADCAST);
+	hif_set_config_data_filter(wvif, true, 0, BIT(1),
+				   BIT(wvif->filter_mcast_count) - 1);
+	hif_set_data_filtering(wvif, true, true);
 }
 
 u64 wfx_prepare_multicast(struct ieee80211_hw *hw,
@@ -213,72 +122,127 @@ u64 wfx_prepare_multicast(struct ieee80211_hw *hw,
 	int count = netdev_hw_addr_list_count(mc_list);
 
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
-		memset(&wvif->mcast_filter, 0x00, sizeof(wvif->mcast_filter));
-		if (!count ||
-		    count > ARRAY_SIZE(wvif->mcast_filter.address_list))
+		if (count > ARRAY_SIZE(wvif->filter_mcast_addr)) {
+			wvif->filter_mcast_count = 0;
 			continue;
+		}
+		wvif->filter_mcast_count = count;
 
 		i = 0;
 		netdev_hw_addr_list_for_each(ha, mc_list) {
-			ether_addr_copy(wvif->mcast_filter.address_list[i],
-					ha->addr);
+			ether_addr_copy(wvif->filter_mcast_addr[i], ha->addr);
 			i++;
 		}
-		wvif->mcast_filter.enable = true;
-		wvif->mcast_filter.num_addresses = count;
 	}
 
 	return 0;
 }
 
-void wfx_configure_filter(struct ieee80211_hw *hw,
-			     unsigned int changed_flags,
-			     unsigned int *total_flags,
-			     u64 unused)
+void wfx_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
+			  unsigned int *total_flags, u64 unused)
 {
 	struct wfx_vif *wvif = NULL;
 	struct wfx_dev *wdev = hw->priv;
+	bool filter_bssid, filter_prbreq, filter_beacon, filter_mcast;
+
+	// Notes:
+	//   - Probe responses (FIF_BCN_PRBRESP_PROMISC) are never filtered
+	//   - PS-Poll (FIF_PSPOLL) are never filtered
+	//   - RTS, CTS and Ack (FIF_CONTROL) are always filtered
+	//   - Broken frames (FIF_FCSFAIL and FIF_PLCPFAIL) are always filtered
+	//   - Firmware does (yet) allow to forward unicast traffic sent to
+	//     other stations (aka. promiscuous mode)
+	*total_flags &= FIF_BCN_PRBRESP_PROMISC | FIF_ALLMULTI | FIF_OTHER_BSS |
+			FIF_PROBE_REQ | FIF_PSPOLL;
 
-	*total_flags &= FIF_OTHER_BSS | FIF_FCSFAIL | FIF_PROBE_REQ;
-
+	mutex_lock(&wdev->conf_mutex);
 	while ((wvif = wvif_iterate(wdev, wvif)) != NULL) {
 		mutex_lock(&wvif->scan_lock);
-		wvif->filter_bssid = (*total_flags &
-				      (FIF_OTHER_BSS | FIF_PROBE_REQ)) ? 0 : 1;
-		wvif->disable_beacon_filter = !(*total_flags & FIF_PROBE_REQ);
-		wfx_fwd_probe_req(wvif, true);
-		wfx_update_filtering(wvif);
+
+		// Note: FIF_BCN_PRBRESP_PROMISC covers probe response and
+		// beacons from other BSS
+		if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
+			filter_beacon = false;
+		else
+			filter_beacon = true;
+		wfx_filter_beacon(wvif, filter_beacon);
+
+		if (*total_flags & FIF_ALLMULTI) {
+			filter_mcast = false;
+		} else if (!wvif->filter_mcast_count) {
+			dev_dbg(wdev->dev, "disabling unconfigured multicast filter");
+			filter_mcast = false;
+		} else {
+			filter_mcast = true;
+		}
+		wfx_filter_mcast(wvif, filter_mcast);
+
+		if (*total_flags & FIF_OTHER_BSS)
+			filter_bssid = false;
+		else
+			filter_bssid = true;
+
+		// In AP mode, chip can reply to probe request itself
+		if (*total_flags & FIF_PROBE_REQ &&
+		    wvif->vif->type == NL80211_IFTYPE_AP) {
+			dev_dbg(wdev->dev, "do not forward probe request in AP mode\n");
+			*total_flags &= ~FIF_PROBE_REQ;
+		}
+
+		if (*total_flags & FIF_PROBE_REQ)
+			filter_prbreq = false;
+		else
+			filter_prbreq = true;
+		hif_set_rx_filter(wvif, filter_bssid, filter_prbreq);
+
 		mutex_unlock(&wvif->scan_lock);
 	}
+	mutex_unlock(&wdev->conf_mutex);
 }
 
-static int wfx_update_pm(struct wfx_vif *wvif)
+int wfx_get_ps_timeout(struct wfx_vif *wvif, bool *enable_ps)
 {
-	struct ieee80211_conf *conf = &wvif->wdev->hw->conf;
-	bool ps = conf->flags & IEEE80211_CONF_PS;
-	int ps_timeout = conf->dynamic_ps_timeout;
 	struct ieee80211_channel *chan0 = NULL, *chan1 = NULL;
+	struct ieee80211_conf *conf = &wvif->wdev->hw->conf;
 
-	WARN_ON(conf->dynamic_ps_timeout < 0);
-	if (wvif->state != WFX_STATE_STA || !wvif->bss_params.aid)
-		return 0;
-	if (!ps)
-		ps_timeout = 0;
-	if (wvif->uapsd_mask)
-		ps_timeout = 0;
-
-	// Kernel disable powersave when an AP is in use. In contrary, it is
-	// absolutely necessary to enable legacy powersave for WF200 if channels
-	// are differents.
+	WARN(!wvif->vif->bss_conf.assoc && enable_ps,
+	     "enable_ps is reliable only if associated");
 	if (wdev_to_wvif(wvif->wdev, 0))
 		chan0 = wdev_to_wvif(wvif->wdev, 0)->vif->bss_conf.chandef.chan;
 	if (wdev_to_wvif(wvif->wdev, 1))
 		chan1 = wdev_to_wvif(wvif->wdev, 1)->vif->bss_conf.chandef.chan;
 	if (chan0 && chan1 && chan0->hw_value != chan1->hw_value &&
 	    wvif->vif->type != NL80211_IFTYPE_AP) {
-		ps = true;
-		ps_timeout = 0;
+		// It is necessary to enable powersave if channels
+		// are differents.
+		if (enable_ps)
+			*enable_ps = true;
+		if (wvif->bss_not_support_ps_poll)
+			return 30;
+		else
+			return 0;
 	}
+	if (enable_ps)
+		*enable_ps = wvif->vif->bss_conf.ps;
+	if (wvif->vif->bss_conf.assoc && wvif->vif->bss_conf.ps)
+		return conf->dynamic_ps_timeout;
+	else
+		return -1;
+}
+
+int wfx_update_pm(struct wfx_vif *wvif)
+{
+	int ps_timeout;
+	bool ps;
+
+	if (!wvif->vif->bss_conf.assoc)
+		return 0;
+	ps_timeout = wfx_get_ps_timeout(wvif, &ps);
+	if (!ps)
+		ps_timeout = 0;
+	WARN_ON(ps_timeout < 0);
+	if (wvif->uapsd_mask)
+		ps_timeout = 0;
 
 	if (!wait_for_completion_timeout(&wvif->set_pm_mode_complete,
 					 TU_TO_JIFFIES(512)))
@@ -287,18 +251,25 @@ static int wfx_update_pm(struct wfx_vif *wvif)
 	return hif_set_pm(wvif, ps, ps_timeout);
 }
 
+static void wfx_update_pm_work(struct work_struct *work)
+{
+	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
+					    update_pm_work);
+
+	wfx_update_pm(wvif);
+}
+
 int wfx_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   u16 queue, const struct ieee80211_tx_queue_params *params)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	int old_uapsd = wvif->uapsd_mask;
 
 	WARN_ON(queue >= hw->queues);
 
 	mutex_lock(&wdev->conf_mutex);
 	assign_bit(queue, &wvif->uapsd_mask, params->uapsd);
-	memcpy(&wvif->edca_params[queue], params, sizeof(*params));
 	hif_set_edca_queue_params(wvif, queue, params);
 	if (wvif->vif->type == NL80211_IFTYPE_STATION &&
 	    old_uapsd != wvif->uapsd_mask) {
@@ -319,32 +290,9 @@ int wfx_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	return 0;
 }
 
-static int __wfx_flush(struct wfx_dev *wdev, bool drop)
-{
-	for (;;) {
-		if (drop)
-			wfx_tx_queues_clear(wdev);
-		if (wait_event_timeout(wdev->tx_queue_stats.wait_link_id_empty,
-				       wfx_tx_queues_is_empty(wdev),
-				       2 * HZ) <= 0)
-			return -ETIMEDOUT;
-		wfx_tx_flush(wdev);
-		if (wfx_tx_queues_is_empty(wdev))
-			return 0;
-		dev_warn(wdev->dev, "frames queued while flushing tx queues");
-	}
-}
-
-void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		  u32 queues, bool drop)
-{
-	// FIXME: only flush requested vif and queues
-	__wfx_flush(hw->priv, drop);
-}
-
 /* WSM callbacks */
 
-static void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
+void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
 {
 	/* RSSI: signed Q8.0, RCPI: unsigned Q7.1
 	 * RSSI = RCPI / 2 - 110
@@ -360,100 +308,23 @@ static void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi)
 	ieee80211_cqm_rssi_notify(wvif->vif, cqm_evt, rcpi_rssi, GFP_KERNEL);
 }
 
-static void wfx_event_handler_work(struct work_struct *work)
+static void wfx_beacon_loss_work(struct work_struct *work)
 {
-	struct wfx_vif *wvif =
-		container_of(work, struct wfx_vif, event_handler_work);
-	struct wfx_hif_event *event;
-
-	LIST_HEAD(list);
-
-	spin_lock(&wvif->event_queue_lock);
-	list_splice_init(&wvif->event_queue, &list);
-	spin_unlock(&wvif->event_queue_lock);
-
-	list_for_each_entry(event, &list, link) {
-		switch (event->evt.event_id) {
-		case HIF_EVENT_IND_BSSLOST:
-			cancel_work_sync(&wvif->unjoin_work);
-			mutex_lock(&wvif->scan_lock);
-			wfx_cqm_bssloss_sm(wvif, 1, 0, 0);
-			mutex_unlock(&wvif->scan_lock);
-			break;
-		case HIF_EVENT_IND_BSSREGAINED:
-			wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-			cancel_work_sync(&wvif->unjoin_work);
-			break;
-		case HIF_EVENT_IND_RCPI_RSSI:
-			wfx_event_report_rssi(wvif,
-					      event->evt.event_data.rcpi_rssi);
-			break;
-		case HIF_EVENT_IND_PS_MODE_ERROR:
-			dev_warn(wvif->wdev->dev,
-				 "error while processing power save request\n");
-			break;
-		default:
-			dev_warn(wvif->wdev->dev,
-				 "unhandled event indication: %.2x\n",
-				 event->evt.event_id);
-			break;
-		}
-	}
-	__wfx_free_event_queue(&list);
-}
-
-static void wfx_bss_loss_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    bss_loss_work.work);
-
-	ieee80211_connection_loss(wvif->vif);
-}
-
-static void wfx_bss_params_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif,
-					    bss_params_work);
+	struct wfx_vif *wvif = container_of(to_delayed_work(work),
+					    struct wfx_vif, beacon_loss_work);
+	struct ieee80211_bss_conf *bss_conf = &wvif->vif->bss_conf;
 
-	mutex_lock(&wvif->wdev->conf_mutex);
-	wvif->bss_params.bss_flags.lost_count_only = 1;
-	hif_set_bss_params(wvif, &wvif->bss_params);
-	wvif->bss_params.bss_flags.lost_count_only = 0;
-	mutex_unlock(&wvif->wdev->conf_mutex);
+	ieee80211_beacon_loss(wvif->vif);
+	schedule_delayed_work(to_delayed_work(work),
+			      msecs_to_jiffies(bss_conf->beacon_int));
 }
 
-static void wfx_do_unjoin(struct wfx_vif *wvif)
+void wfx_set_default_unicast_key(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif, int idx)
 {
-	mutex_lock(&wvif->wdev->conf_mutex);
-
-	if (!wvif->state)
-		goto done;
-
-	if (wvif->state == WFX_STATE_AP)
-		goto done;
-
-	cancel_work_sync(&wvif->update_filtering_work);
-	wvif->state = WFX_STATE_PASSIVE;
-
-	/* Unjoin is a reset. */
-	wfx_tx_flush(wvif->wdev);
-	hif_keep_alive_period(wvif, 0);
-	hif_reset(wvif, false);
-	wfx_tx_policy_init(wvif);
-	hif_set_macaddr(wvif, wvif->vif->addr);
-	wfx_free_event_queue(wvif);
-	cancel_work_sync(&wvif->event_handler_work);
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	/* Disable Block ACKs */
-	hif_set_block_ack_policy(wvif, 0, 0);
-
-	wvif->disable_beacon_filter = false;
-	wfx_update_filtering(wvif);
-	memset(&wvif->bss_params, 0, sizeof(wvif->bss_params));
-
-done:
-	mutex_unlock(&wvif->wdev->conf_mutex);
+	hif_wep_default_key_id(wvif, idx);
 }
 
 static void wfx_set_mfp(struct wfx_vif *wvif,
@@ -472,8 +343,7 @@ static void wfx_set_mfp(struct wfx_vif *wvif,
 
 	rcu_read_lock();
 	if (bss)
-		ptr = (const u16 *) ieee80211_bss_get_ie(bss,
-							      WLAN_EID_RSN);
+		ptr = (const u16 *)ieee80211_bss_get_ie(bss, WLAN_EID_RSN);
 
 	if (ptr) {
 		ptr += pairwise_cipher_suite_count_offset;
@@ -487,6 +357,24 @@ static void wfx_set_mfp(struct wfx_vif *wvif,
 	hif_set_mfp(wvif, mfpc, mfpr);
 }
 
+void wfx_reset(struct wfx_vif *wvif)
+{
+	struct wfx_dev *wdev = wvif->wdev;
+
+	wfx_tx_lock_flush(wdev);
+	hif_reset(wvif, false);
+	wfx_tx_policy_init(wvif);
+	if (wvif_count(wdev) <= 1)
+		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
+	wfx_tx_unlock(wdev);
+	wvif->join_in_progress = false;
+	wvif->bss_not_support_ps_poll = false;
+	cancel_delayed_work_sync(&wvif->beacon_loss_work);
+	wvif =  NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
+		wfx_update_pm(wvif);
+}
+
 static void wfx_do_join(struct wfx_vif *wvif)
 {
 	int ret;
@@ -498,9 +386,6 @@ static void wfx_do_join(struct wfx_vif *wvif)
 
 	wfx_tx_lock_flush(wvif->wdev);
 
-	if (wvif->state)
-		wfx_do_unjoin(wvif);
-
 	bss = cfg80211_get_bss(wvif->wdev->hw->wiphy, wvif->channel,
 			       conf->bssid, NULL, 0,
 			       IEEE80211_BSS_TYPE_ANY, IEEE80211_PRIVACY_ANY);
@@ -509,109 +394,72 @@ static void wfx_do_join(struct wfx_vif *wvif)
 		return;
 	}
 
-	mutex_lock(&wvif->wdev->conf_mutex);
-
-	/* Sanity check beacon interval */
-	if (!wvif->beacon_int)
-		wvif->beacon_int = 1;
-
 	rcu_read_lock(); // protect ssidie
-	if (!conf->ibss_joined)
+	if (bss)
 		ssidie = ieee80211_bss_get_ie(bss, WLAN_EID_SSID);
 	if (ssidie) {
 		ssidlen = ssidie[1];
-		memcpy(ssid, &ssidie[2], ssidie[1]);
+		if (ssidlen > IEEE80211_MAX_SSID_LEN)
+			ssidlen = IEEE80211_MAX_SSID_LEN;
+		memcpy(ssid, &ssidie[2], ssidlen);
 	}
 	rcu_read_unlock();
 
-	wfx_tx_flush(wvif->wdev);
-
-	if (wvif_count(wvif->wdev) <= 1)
-		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
-
 	wfx_set_mfp(wvif, bss);
+	cfg80211_put_bss(wvif->wdev->hw->wiphy, bss);
 
-	wvif->wdev->tx_burst_idx = -1;
+	wvif->join_in_progress = true;
 	ret = hif_join(wvif, conf, wvif->channel, ssid, ssidlen);
 	if (ret) {
 		ieee80211_connection_loss(wvif->vif);
-		wvif->join_complete_status = -1;
-		/* Tx lock still held, unjoin will clear it. */
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wvif->wdev);
+		wfx_reset(wvif);
 	} else {
-		wvif->join_complete_status = 0;
-		if (wvif->vif->type == NL80211_IFTYPE_ADHOC)
-			wvif->state = WFX_STATE_IBSS;
-		else
-			wvif->state = WFX_STATE_PRE_STA;
-		wfx_tx_unlock(wvif->wdev);
-
-		/* Upload keys */
-		wfx_upload_keys(wvif);
-
 		/* Due to beacon filtering it is possible that the
 		 * AP's beacon is not known for the mac80211 stack.
 		 * Disable filtering temporary to make sure the stack
 		 * receives at least one
 		 */
-		wvif->disable_beacon_filter = true;
+		wfx_filter_beacon(wvif, false);
 	}
-	wfx_update_filtering(wvif);
-
-	mutex_unlock(&wvif->wdev->conf_mutex);
-	if (bss)
-		cfg80211_put_bss(wvif->wdev->hw->wiphy, bss);
-}
-
-static void wfx_unjoin_work(struct work_struct *work)
-{
-	struct wfx_vif *wvif = container_of(work, struct wfx_vif, unjoin_work);
-
-	wfx_do_unjoin(wvif);
 	wfx_tx_unlock(wvif->wdev);
 }
 
 int wfx_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		struct ieee80211_sta *sta)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
 
 	spin_lock_init(&sta_priv->lock);
 	sta_priv->vif_id = wvif->id;
 
-	// FIXME: in station mode, the current API interprets new link-id as a
-	// tdls peer.
-	if (vif->type == NL80211_IFTYPE_STATION)
+	// In station mode, the firmware interprets new link-id as a TDLS peer.
+	if (vif->type == NL80211_IFTYPE_STATION && !sta->tdls)
 		return 0;
 	sta_priv->link_id = ffz(wvif->link_id_map);
 	wvif->link_id_map |= BIT(sta_priv->link_id);
 	WARN_ON(!sta_priv->link_id);
-	WARN_ON(sta_priv->link_id >= WFX_MAX_STA_IN_AP_MODE);
+	WARN_ON(sta_priv->link_id >= HIF_LINK_ID_MAX);
 	hif_map_link(wvif, sta->addr, 0, sta_priv->link_id);
 
-	spin_lock_bh(&wvif->ps_state_lock);
-	if ((sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) ==
-					IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK)
-		wvif->sta_asleep_mask |= BIT(sta_priv->link_id);
-	spin_unlock_bh(&wvif->ps_state_lock);
 	return 0;
 }
 
 int wfx_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		   struct ieee80211_sta *sta)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *)&sta->drv_priv;
 	int i;
 
 	for (i = 0; i < ARRAY_SIZE(sta_priv->buffered); i++)
 		if (sta_priv->buffered[i])
-			dev_warn(wvif->wdev->dev, "release station while %d pending frame on queue %d",
-				 sta_priv->buffered[i], i);
-	// FIXME: see note in wfx_sta_add()
-	if (vif->type == NL80211_IFTYPE_STATION)
+			// Not an error if paired with trace in
+			// wfx_tx_update_sta()
+			dev_dbg(wvif->wdev->dev, "release station while %d pending frame on queue %d",
+				sta_priv->buffered[i], i);
+	// See note in wfx_sta_add()
+	if (!sta_priv->link_id)
 		return 0;
 	// FIXME add a mutex?
 	hif_map_link(wvif, sta->addr, 1, sta_priv->link_id);
@@ -619,50 +467,10 @@ int wfx_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	return 0;
 }
 
-static int wfx_start_ap(struct wfx_vif *wvif)
-{
-	int ret;
-
-	wvif->beacon_int = wvif->vif->bss_conf.beacon_int;
-	wvif->wdev->tx_burst_idx = -1;
-	ret = hif_start(wvif, &wvif->vif->bss_conf, wvif->channel);
-	if (ret)
-		return ret;
-	ret = wfx_upload_keys(wvif);
-	if (ret)
-		return ret;
-	if (wvif_count(wvif->wdev) <= 1)
-		hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
-	wvif->state = WFX_STATE_AP;
-	wfx_update_filtering(wvif);
-	return 0;
-}
-
-static int wfx_update_beaconing(struct wfx_vif *wvif)
-{
-	if (wvif->vif->type != NL80211_IFTYPE_AP)
-		return 0;
-	if (wvif->state == WFX_STATE_AP &&
-	    wvif->beacon_int == wvif->vif->bss_conf.beacon_int)
-		return 0;
-	wfx_tx_lock_flush(wvif->wdev);
-	hif_reset(wvif, false);
-	wfx_tx_policy_init(wvif);
-	wvif->state = WFX_STATE_PASSIVE;
-	wfx_start_ap(wvif);
-	wfx_tx_unlock(wvif->wdev);
-	return 0;
-}
-
 static int wfx_upload_ap_templates(struct wfx_vif *wvif)
 {
 	struct sk_buff *skb;
 
-	if (wvif->vif->type == NL80211_IFTYPE_STATION ||
-	    wvif->vif->type == NL80211_IFTYPE_MONITOR ||
-	    wvif->vif->type == NL80211_IFTYPE_UNSPECIFIED)
-		return 0;
-
 	skb = ieee80211_beacon_get(wvif->wdev->hw, wvif->vif);
 	if (!skb)
 		return -ENOMEM;
@@ -679,52 +487,77 @@ static int wfx_upload_ap_templates(struct wfx_vif *wvif)
 	return 0;
 }
 
+int wfx_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+	struct wfx_dev *wdev = wvif->wdev;
+	int ret;
+
+	wvif =  NULL;
+	while ((wvif = wvif_iterate(wdev, wvif)) != NULL)
+		wfx_update_pm(wvif);
+	wvif = (struct wfx_vif *)vif->drv_priv;
+	wfx_upload_ap_templates(wvif);
+	ret = hif_start(wvif, &vif->bss_conf, wvif->channel);
+	if (ret > 0)
+		return -EIO;
+	return ret;
+}
+
+void wfx_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
+
+	wfx_reset(wvif);
+}
+
 static void wfx_join_finalize(struct wfx_vif *wvif,
 			      struct ieee80211_bss_conf *info)
 {
-	struct ieee80211_sta *sta = NULL;
-
-	wvif->beacon_int = info->beacon_int;
-	rcu_read_lock(); // protect sta
-	if (info->bssid && !info->ibss_joined)
-		sta = ieee80211_find_sta(wvif->vif, info->bssid);
-	if (sta)
-		wvif->bss_params.operational_rate_set =
-			wfx_rate_mask_to_hw(wvif->wdev, sta->supp_rates[wvif->channel->band]);
-	else
-		wvif->bss_params.operational_rate_set = -1;
-	rcu_read_unlock();
-	if (sta &&
-	    info->ht_operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT)
-		hif_dual_cts_protection(wvif, true);
-	else
-		hif_dual_cts_protection(wvif, false);
+	wvif->join_in_progress = false;
+	hif_set_association_mode(wvif, info);
+	hif_keep_alive_period(wvif, 0);
+	// beacon_loss_count is defined to 7 in net/mac80211/mlme.c. Let's use
+	// the same value.
+	hif_set_bss_params(wvif, info->aid, 7);
+	hif_set_beacon_wakeup_period(wvif, 1, 1);
+	wfx_update_pm(wvif);
+}
 
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-	cancel_work_sync(&wvif->unjoin_work);
+int wfx_join_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	wvif->bss_params.beacon_lost_count = 20;
-	wvif->bss_params.aid = info->aid;
+	wfx_upload_ap_templates(wvif);
+	wfx_do_join(wvif);
+	return 0;
+}
 
-	hif_set_association_mode(wvif, info);
+void wfx_leave_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
-	if (!info->ibss_joined) {
-		hif_keep_alive_period(wvif, 30 /* sec */);
-		hif_set_bss_params(wvif, &wvif->bss_params);
-		hif_set_beacon_wakeup_period(wvif, info->dtim_period,
-					     info->dtim_period);
-		wfx_update_pm(wvif);
+	wfx_reset(wvif);
+}
+
+static void wfx_enable_beacon(struct wfx_vif *wvif, bool enable)
+{
+	// Driver has Content After DTIM Beacon in queue. Driver is waiting for
+	// a signal from the firmware. Since we are going to stop to send
+	// beacons, this signal will never happens. See also
+	// wfx_suspend_resume_mc()
+	if (!enable && wfx_tx_queues_has_cab(wvif)) {
+		wvif->after_dtim_tx_allowed = true;
+		wfx_bh_request_tx(wvif->wdev);
 	}
+	hif_beacon_transmit(wvif, enable);
 }
 
-void wfx_bss_info_changed(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif,
-			     struct ieee80211_bss_conf *info,
-			     u32 changed)
+void wfx_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+			  struct ieee80211_bss_conf *info, u32 changed)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	bool do_join = false;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	int i;
 
 	mutex_lock(&wdev->conf_mutex);
@@ -742,92 +575,52 @@ void wfx_bss_info_changed(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (changed & BSS_CHANGED_BEACON ||
-	    changed & BSS_CHANGED_AP_PROBE_RESP ||
-	    changed & BSS_CHANGED_BSSID ||
-	    changed & BSS_CHANGED_SSID ||
-	    changed & BSS_CHANGED_IBSS) {
-		wvif->beacon_int = info->beacon_int;
-		wfx_update_beaconing(wvif);
-		wfx_upload_ap_templates(wvif);
-		wfx_fwd_probe_req(wvif, false);
+	if (changed & BSS_CHANGED_BASIC_RATES ||
+	    changed & BSS_CHANGED_BEACON_INT ||
+	    changed & BSS_CHANGED_BSSID) {
+		if (vif->type == NL80211_IFTYPE_STATION)
+			wfx_do_join(wvif);
 	}
 
-	if (changed & BSS_CHANGED_BEACON_ENABLED &&
-	    wvif->state != WFX_STATE_IBSS)
-		hif_beacon_transmit(wvif, info->enable_beacon);
+	if (changed & BSS_CHANGED_AP_PROBE_RESP ||
+	    changed & BSS_CHANGED_BEACON)
+		wfx_upload_ap_templates(wvif);
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED)
+		wfx_enable_beacon(wvif, info->enable_beacon);
 
-	if (changed & BSS_CHANGED_BEACON_INFO)
+	if (changed & BSS_CHANGED_BEACON_INFO) {
+		if (vif->type != NL80211_IFTYPE_STATION)
+			dev_warn(wdev->dev, "%s: misunderstood change: BEACON_INFO\n",
+				 __func__);
 		hif_set_beacon_wakeup_period(wvif, info->dtim_period,
 					     info->dtim_period);
-
-	/* assoc/disassoc, or maybe AID changed */
-	if (changed & BSS_CHANGED_ASSOC) {
-		wfx_tx_lock_flush(wdev);
-		wvif->wep_default_key_id = -1;
-		wfx_tx_unlock(wdev);
+		// We temporary forwarded beacon for join process. It is now no
+		// more necessary.
+		wfx_filter_beacon(wvif, true);
 	}
 
-	if (changed & BSS_CHANGED_ASSOC && !info->assoc &&
-	    (wvif->state == WFX_STATE_STA || wvif->state == WFX_STATE_IBSS)) {
-		/* Shedule unjoin work */
-		wfx_tx_lock(wdev);
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wdev);
-	} else {
-		if (changed & BSS_CHANGED_BEACON_INT) {
-			if (info->ibss_joined)
-				do_join = true;
-			else if (wvif->state == WFX_STATE_AP)
-				wfx_update_beaconing(wvif);
-		}
-
-		if (changed & BSS_CHANGED_BSSID)
-			do_join = true;
-
-		if (changed & BSS_CHANGED_ASSOC ||
-		    changed & BSS_CHANGED_BSSID ||
-		    changed & BSS_CHANGED_IBSS ||
-		    changed & BSS_CHANGED_BASIC_RATES ||
-		    changed & BSS_CHANGED_HT) {
-			if (info->assoc) {
-				if (wvif->state < WFX_STATE_PRE_STA) {
-					ieee80211_connection_loss(vif);
-					mutex_unlock(&wdev->conf_mutex);
-					return;
-				} else if (wvif->state == WFX_STATE_PRE_STA) {
-					wvif->state = WFX_STATE_STA;
-				}
-			} else {
-				do_join = true;
-			}
-
-			if (info->assoc || info->ibss_joined)
-				wfx_join_finalize(wvif, info);
-			else
-				memset(&wvif->bss_params, 0,
-				       sizeof(wvif->bss_params));
-		}
+	if (changed & BSS_CHANGED_ASSOC) {
+		if (info->assoc || info->ibss_joined)
+			wfx_join_finalize(wvif, info);
+		else if (!info->assoc && vif->type == NL80211_IFTYPE_STATION)
+			wfx_reset(wvif);
+		else
+			dev_warn(wdev->dev, "%s: misunderstood change: ASSOC\n",
+				 __func__);
 	}
 
-	if (changed & BSS_CHANGED_ASSOC ||
-	    changed & BSS_CHANGED_ERP_CTS_PROT ||
-	    changed & BSS_CHANGED_ERP_PREAMBLE) {
-		u8 erp_ie[3] = { WLAN_EID_ERP_INFO, 1, 0 };
+	if (changed & BSS_CHANGED_KEEP_ALIVE)
+		hif_keep_alive_period(wvif, info->max_idle_period *
+					    USEC_PER_TU / USEC_PER_MSEC);
 
+	if (changed & BSS_CHANGED_ERP_CTS_PROT)
 		hif_erp_use_protection(wvif, info->use_cts_prot);
-		if (info->use_cts_prot)
-			erp_ie[2] |= WLAN_ERP_USE_PROTECTION;
-		if (info->use_short_preamble)
-			erp_ie[2] |= WLAN_ERP_BARKER_PREAMBLE;
-		if (wvif->vif->type != NL80211_IFTYPE_STATION)
-			hif_update_ie_beacon(wvif, erp_ie, sizeof(erp_ie));
-	}
 
-	if (changed & BSS_CHANGED_ASSOC || changed & BSS_CHANGED_ERP_SLOT)
+	if (changed & BSS_CHANGED_ERP_SLOT)
 		hif_slot_time(wvif, info->use_short_slot ? 9 : 20);
 
-	if (changed & BSS_CHANGED_ASSOC || changed & BSS_CHANGED_CQM)
+	if (changed & BSS_CHANGED_CQM)
 		hif_set_rcpi_rssi_threshold(wvif, info->cqm_rssi_thold,
 					    info->cqm_rssi_hyst);
 
@@ -838,31 +631,6 @@ void wfx_bss_info_changed(struct ieee80211_hw *hw,
 		wfx_update_pm(wvif);
 
 	mutex_unlock(&wdev->conf_mutex);
-
-	if (do_join)
-		wfx_do_join(wvif);
-}
-
-static void wfx_ps_notify_sta(struct wfx_vif *wvif,
-			      enum sta_notify_cmd notify_cmd, int link_id)
-{
-	spin_lock_bh(&wvif->ps_state_lock);
-	if (notify_cmd == STA_NOTIFY_SLEEP)
-		wvif->sta_asleep_mask |= BIT(link_id);
-	else // notify_cmd == STA_NOTIFY_AWAKE
-		wvif->sta_asleep_mask &= ~BIT(link_id);
-	spin_unlock_bh(&wvif->ps_state_lock);
-	if (notify_cmd == STA_NOTIFY_AWAKE)
-		wfx_bh_request_tx(wvif->wdev);
-}
-
-void wfx_sta_notify(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		    enum sta_notify_cmd notify_cmd, struct ieee80211_sta *sta)
-{
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
-	struct wfx_sta_priv *sta_priv = (struct wfx_sta_priv *) &sta->drv_priv;
-
-	wfx_ps_notify_sta(wvif, notify_cmd, sta_priv->link_id);
 }
 
 static int wfx_update_tim(struct wfx_vif *wvif)
@@ -873,10 +641,8 @@ static int wfx_update_tim(struct wfx_vif *wvif)
 
 	skb = ieee80211_beacon_get_tim(wvif->wdev->hw, wvif->vif,
 				       &tim_offset, &tim_length);
-	if (!skb) {
-		__wfx_flush(wvif->wdev, true);
+	if (!skb)
 		return -ENOENT;
-	}
 	tim_ptr = skb->data + tim_offset;
 
 	if (tim_offset && tim_length >= 6) {
@@ -886,7 +652,7 @@ static int wfx_update_tim(struct wfx_vif *wvif)
 		tim_ptr[2] = 0;
 
 		/* Set/reset aid0 bit */
-		if (wfx_tx_queues_get_after_dtim(wvif))
+		if (wfx_tx_queues_has_cab(wvif))
 			tim_ptr[4] |= 1;
 		else
 			tim_ptr[4] &= ~1;
@@ -917,7 +683,9 @@ int wfx_set_tim(struct ieee80211_hw *hw, struct ieee80211_sta *sta, bool set)
 
 void wfx_suspend_resume_mc(struct wfx_vif *wvif, enum sta_notify_cmd notify_cmd)
 {
-	WARN(!wfx_tx_queues_get_after_dtim(wvif), "incorrect sequence");
+	if (notify_cmd != STA_NOTIFY_AWAKE)
+		return;
+	WARN(!wfx_tx_queues_has_cab(wvif), "incorrect sequence");
 	WARN(wvif->after_dtim_tx_allowed, "incorrect sequence");
 	wvif->after_dtim_tx_allowed = true;
 	wfx_bh_request_tx(wvif->wdev);
@@ -958,7 +726,7 @@ void wfx_change_chanctx(struct ieee80211_hw *hw,
 int wfx_assign_vif_chanctx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			   struct ieee80211_chanctx_conf *conf)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct ieee80211_channel *ch = conf->def.chan;
 
 	WARN(wvif->channel, "channel overwrite");
@@ -971,7 +739,7 @@ void wfx_unassign_vif_chanctx(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      struct ieee80211_chanctx_conf *conf)
 {
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 	struct ieee80211_channel *ch = conf->def.chan;
 
 	WARN(wvif->channel != ch, "channel mismatch");
@@ -987,7 +755,7 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	int i, ret = 0;
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
 			     IEEE80211_VIF_SUPPORTS_UAPSD |
@@ -1021,33 +789,18 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	wvif->wdev = wdev;
 
 	wvif->link_id_map = 1; // link-id 0 is reserved for multicast
-	spin_lock_init(&wvif->ps_state_lock);
 	INIT_WORK(&wvif->update_tim_work, wfx_update_tim_work);
-
-	memset(&wvif->bss_params, 0, sizeof(wvif->bss_params));
-
-	mutex_init(&wvif->bss_loss_lock);
-	INIT_DELAYED_WORK(&wvif->bss_loss_work, wfx_bss_loss_work);
-
-	wvif->wep_default_key_id = -1;
-	INIT_WORK(&wvif->wep_key_work, wfx_wep_key_work);
-
-	spin_lock_init(&wvif->event_queue_lock);
-	INIT_LIST_HEAD(&wvif->event_queue);
-	INIT_WORK(&wvif->event_handler_work, wfx_event_handler_work);
+	INIT_DELAYED_WORK(&wvif->beacon_loss_work, wfx_beacon_loss_work);
 
 	init_completion(&wvif->set_pm_mode_complete);
 	complete(&wvif->set_pm_mode_complete);
-	INIT_WORK(&wvif->update_filtering_work, wfx_update_filtering_work);
-	INIT_WORK(&wvif->bss_params_work, wfx_bss_params_work);
-	INIT_WORK(&wvif->unjoin_work, wfx_unjoin_work);
+	INIT_WORK(&wvif->update_pm_work, wfx_update_pm_work);
 	INIT_WORK(&wvif->tx_policy_upload_work, wfx_tx_policy_upload_work);
 
 	mutex_init(&wvif->scan_lock);
 	init_completion(&wvif->scan_complete);
 	INIT_WORK(&wvif->scan_work, wfx_hw_scan_work);
 
-	INIT_WORK(&wvif->tx_policy_upload_work, wfx_tx_policy_upload_work);
 	mutex_unlock(&wdev->conf_mutex);
 
 	hif_set_macaddr(wvif, vif->addr);
@@ -1060,50 +813,25 @@ int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 			hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
 		else
 			hif_set_block_ack_policy(wvif, 0x00, 0x00);
-		// Combo force powersave mode. We can re-enable it now
-		ret = wfx_update_pm(wvif);
 	}
 	return ret;
 }
 
-void wfx_remove_interface(struct ieee80211_hw *hw,
-			  struct ieee80211_vif *vif)
+void wfx_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct wfx_dev *wdev = hw->priv;
-	struct wfx_vif *wvif = (struct wfx_vif *) vif->drv_priv;
+	struct wfx_vif *wvif = (struct wfx_vif *)vif->drv_priv;
 
 	wait_for_completion_timeout(&wvif->set_pm_mode_complete, msecs_to_jiffies(300));
 
 	mutex_lock(&wdev->conf_mutex);
 	WARN(wvif->link_id_map != 1, "corrupted state");
-	switch (wvif->state) {
-	case WFX_STATE_PRE_STA:
-	case WFX_STATE_STA:
-	case WFX_STATE_IBSS:
-		wfx_tx_lock_flush(wdev);
-		if (!schedule_work(&wvif->unjoin_work))
-			wfx_tx_unlock(wdev);
-		break;
-	case WFX_STATE_AP:
-		wvif->sta_asleep_mask = 0;
-		/* reset.link_id = 0; */
-		hif_reset(wvif, false);
-		break;
-	default:
-		break;
-	}
-
-	wvif->state = WFX_STATE_PASSIVE;
-	wfx_tx_queues_wait_empty_vif(wvif);
-	wfx_tx_unlock(wdev);
 
-	/* FIXME: In add to reset MAC address, try to reset interface */
+	hif_reset(wvif, false);
 	hif_set_macaddr(wvif, NULL);
+	wfx_tx_policy_init(wvif);
 
-	wfx_cqm_bssloss_sm(wvif, 0, 0, 0);
-	cancel_work_sync(&wvif->unjoin_work);
-	wfx_free_event_queue(wvif);
-
+	cancel_delayed_work_sync(&wvif->beacon_loss_work);
 	wdev->vif[wvif->id] = NULL;
 	wvif->vif = NULL;
 
@@ -1115,8 +843,6 @@ void wfx_remove_interface(struct ieee80211_hw *hw,
 			hif_set_block_ack_policy(wvif, 0xFF, 0xFF);
 		else
 			hif_set_block_ack_policy(wvif, 0x00, 0x00);
-		// Combo force powersave mode. We can re-enable it now
-		wfx_update_pm(wvif);
 	}
 }
 
@@ -1129,10 +855,5 @@ void wfx_stop(struct ieee80211_hw *hw)
 {
 	struct wfx_dev *wdev = hw->priv;
 
-	wfx_tx_lock_flush(wdev);
-	mutex_lock(&wdev->conf_mutex);
-	wfx_tx_queues_clear(wdev);
-	mutex_unlock(&wdev->conf_mutex);
-	wfx_tx_unlock(wdev);
-	WARN(atomic_read(&wdev->tx_lock), "tx_lock is locked");
+	wfx_tx_queues_check_empty(wdev);
 }
diff --git a/drivers/staging/wfx/sta.h b/drivers/staging/wfx/sta.h
index cf99a8a74a81..8a20ad9ae017 100644
--- a/drivers/staging/wfx/sta.h
+++ b/drivers/staging/wfx/sta.h
@@ -10,34 +10,13 @@
 
 #include <net/mac80211.h>
 
-#include "hif_api_cmd.h"
-
 struct wfx_dev;
 struct wfx_vif;
 
-enum wfx_state {
-	WFX_STATE_PASSIVE = 0,
-	WFX_STATE_PRE_STA,
-	WFX_STATE_STA,
-	WFX_STATE_IBSS,
-	WFX_STATE_AP,
-};
-
-struct wfx_hif_event {
-	struct list_head link;
-	struct hif_ind_event evt;
-};
-
-struct wfx_grp_addr_table {
-	bool enable;
-	int num_addresses;
-	u8 address_list[8][ETH_ALEN];
-};
-
 struct wfx_sta_priv {
 	int link_id;
 	int vif_id;
-	u8 buffered[IEEE80211_NUM_TIDS];
+	int buffered[IEEE80211_NUM_TIDS];
 	// Ensure atomicity of "buffered" and calls to ieee80211_sta_set_buffered()
 	spinlock_t lock;
 };
@@ -47,6 +26,8 @@ int wfx_start(struct ieee80211_hw *hw);
 void wfx_stop(struct ieee80211_hw *hw);
 int wfx_config(struct ieee80211_hw *hw, u32 changed);
 int wfx_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
+void wfx_set_default_unicast_key(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif, int idx);
 u64 wfx_prepare_multicast(struct ieee80211_hw *hw,
 			  struct netdev_hw_addr_list *mc_list);
 void wfx_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
@@ -54,8 +35,10 @@ void wfx_configure_filter(struct ieee80211_hw *hw, unsigned int changed_flags,
 
 int wfx_add_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
 void wfx_remove_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
-void wfx_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-	       u32 queues, bool drop);
+int wfx_start_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+void wfx_stop_ap(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+int wfx_join_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+void wfx_leave_ibss(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
 int wfx_conf_tx(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		u16 queue, const struct ieee80211_tx_queue_params *params);
 void wfx_bss_info_changed(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
@@ -82,12 +65,13 @@ void wfx_unassign_vif_chanctx(struct ieee80211_hw *hw,
 			      struct ieee80211_chanctx_conf *conf);
 
 // WSM Callbacks
+void wfx_cooling_timeout_work(struct work_struct *work);
+void wfx_suspend_hot_dev(struct wfx_dev *wdev, enum sta_notify_cmd cmd);
 void wfx_suspend_resume_mc(struct wfx_vif *wvif, enum sta_notify_cmd cmd);
+void wfx_event_report_rssi(struct wfx_vif *wvif, u8 raw_rcpi_rssi);
 
 // Other Helpers
-void wfx_cqm_bssloss_sm(struct wfx_vif *wvif, int init, int good, int bad);
-void wfx_update_filtering(struct wfx_vif *wvif);
-int wfx_fwd_probe_req(struct wfx_vif *wvif, bool enable);
+void wfx_reset(struct wfx_vif *wvif);
 u32 wfx_rate_mask_to_hw(struct wfx_dev *wdev, u32 rates);
 
 #endif /* WFX_STA_H */
diff --git a/drivers/staging/wfx/traces.h b/drivers/staging/wfx/traces.h
index 30c6a13f0e22..0b6fbd518638 100644
--- a/drivers/staging/wfx/traces.h
+++ b/drivers/staging/wfx/traces.h
@@ -32,16 +32,16 @@
  *             xxx_name(XXX)       \
  *             ...
  *
- *   3. Instanciate that list_names:
+ *   3. Instantiate that list_names:
  *
  *          list_names
  *
- *   4. Redefine xxx_name() as a entry of array for __print_symbolic()
+ *   4. Redefine xxx_name() as an entry of array for __print_symbolic()
  *
  *          #undef xxx_name
  *          #define xxx_name(msg) { msg, #msg },
  *
- *   5. list_name can now nearlu be used with __print_symbolic() but,
+ *   5. list_name can now nearly be used with __print_symbolic() but,
  *      __print_symbolic() dislike last comma of list. So we define a new list
  *      with a dummy element:
  *
@@ -104,8 +104,10 @@ hif_msg_list_enum
 	hif_mib_name(ARP_KEEP_ALIVE_PERIOD)          \
 	hif_mib_name(BEACON_FILTER_ENABLE)           \
 	hif_mib_name(BEACON_FILTER_TABLE)            \
+	hif_mib_name(BEACON_STATS)                   \
 	hif_mib_name(BEACON_WAKEUP_PERIOD)           \
 	hif_mib_name(BLOCK_ACK_POLICY)               \
+	hif_mib_name(CCA_CONFIG)                     \
 	hif_mib_name(CONFIG_DATA_FILTER)             \
 	hif_mib_name(COUNTERS_TABLE)                 \
 	hif_mib_name(CURRENT_TX_POWER_LEVEL)         \
@@ -114,29 +116,32 @@ hif_msg_list_enum
 	hif_mib_name(DOT11_MAX_TRANSMIT_MSDU_LIFETIME) \
 	hif_mib_name(DOT11_RTS_THRESHOLD)            \
 	hif_mib_name(DOT11_WEP_DEFAULT_KEY_ID)       \
+	hif_mib_name(ETHERTYPE_DATAFRAME_CONDITION)  \
+	hif_mib_name(EXTENDED_COUNTERS_TABLE)        \
 	hif_mib_name(GL_BLOCK_ACK_INFO)              \
 	hif_mib_name(GL_OPERATIONAL_POWER_MODE)      \
 	hif_mib_name(GL_SET_MULTI_MSG)               \
+	hif_mib_name(GRP_SEQ_COUNTER)                \
 	hif_mib_name(INACTIVITY_TIMER)               \
 	hif_mib_name(INTERFACE_PROTECTION)           \
 	hif_mib_name(IPV4_ADDR_DATAFRAME_CONDITION)  \
 	hif_mib_name(IPV6_ADDR_DATAFRAME_CONDITION)  \
 	hif_mib_name(KEEP_ALIVE_PERIOD)              \
 	hif_mib_name(MAC_ADDR_DATAFRAME_CONDITION)   \
+	hif_mib_name(MAGIC_DATAFRAME_CONDITION)      \
+	hif_mib_name(MAX_TX_POWER_LEVEL)             \
 	hif_mib_name(NON_ERP_PROTECTION)             \
 	hif_mib_name(NS_IP_ADDRESSES_TABLE)          \
 	hif_mib_name(OVERRIDE_INTERNAL_TX_RATE)      \
+	hif_mib_name(PORT_DATAFRAME_CONDITION)       \
 	hif_mib_name(PROTECTED_MGMT_POLICY)          \
-	hif_mib_name(RX_FILTER)                      \
 	hif_mib_name(RCPI_RSSI_THRESHOLD)            \
+	hif_mib_name(RX_FILTER)                      \
 	hif_mib_name(SET_ASSOCIATION_MODE)           \
 	hif_mib_name(SET_DATA_FILTERING)             \
-	hif_mib_name(ETHERTYPE_DATAFRAME_CONDITION)  \
 	hif_mib_name(SET_HT_PROTECTION)              \
-	hif_mib_name(MAGIC_DATAFRAME_CONDITION)      \
 	hif_mib_name(SET_TX_RATE_RETRY_POLICY)       \
 	hif_mib_name(SET_UAPSD_INFORMATION)          \
-	hif_mib_name(PORT_DATAFRAME_CONDITION)       \
 	hif_mib_name(SLOT_TIME)                      \
 	hif_mib_name(STATISTICS_TABLE)               \
 	hif_mib_name(TEMPLATE_FRAME)                 \
@@ -169,7 +174,7 @@ DECLARE_EVENT_CLASS(hif_data,
 		int header_len;
 
 		__entry->tx_fill_level = tx_fill_level;
-		__entry->msg_len = hif->len;
+		__entry->msg_len = le16_to_cpu(hif->len);
 		__entry->msg_id = hif->id;
 		__entry->if_id = hif->interface;
 		if (is_recv)
@@ -179,7 +184,7 @@ DECLARE_EVENT_CLASS(hif_data,
 		if (!is_recv &&
 		    (__entry->msg_id == HIF_REQ_ID_READ_MIB ||
 		     __entry->msg_id == HIF_REQ_ID_WRITE_MIB)) {
-			__entry->mib = le16_to_cpup((u16 *) hif->body);
+			__entry->mib = le16_to_cpup((__le16 *)hif->body);
 			header_len = 4;
 		} else {
 			__entry->mib = -1;
@@ -193,8 +198,8 @@ DECLARE_EVENT_CLASS(hif_data,
 	TP_printk("%d:%d:%s_%s%s%s: %s%s (%d bytes)",
 		__entry->tx_fill_level,
 		__entry->if_id,
-		__print_symbolic(__entry->msg_id, hif_msg_list),
 		__entry->msg_type,
+		__print_symbolic(__entry->msg_id, hif_msg_list),
 		__entry->mib != -1 ? "/" : "",
 		__entry->mib != -1 ? __print_symbolic(__entry->mib, hif_mib_list) : "",
 		__print_hex(__entry->buf, __entry->buf_len),
@@ -382,8 +387,8 @@ TRACE_EVENT(tx_stats,
 		int i;
 
 		__entry->pkt_id = tx_cnf->packet_id;
-		__entry->delay_media = tx_cnf->media_delay;
-		__entry->delay_queue = tx_cnf->tx_queue_delay;
+		__entry->delay_media = le32_to_cpu(tx_cnf->media_delay);
+		__entry->delay_queue = le32_to_cpu(tx_cnf->tx_queue_delay);
 		__entry->delay_fw = delay;
 		__entry->ack_failures = tx_cnf->ack_failures;
 		if (!tx_cnf->status || __entry->ack_failures)
@@ -409,7 +414,7 @@ TRACE_EVENT(tx_stats,
 			__entry->flags |= 0x10;
 		if (tx_cnf->status)
 			__entry->flags |= 0x20;
-		if (tx_cnf->status == HIF_REQUEUE)
+		if (tx_cnf->status == HIF_STATUS_TX_FAIL_REQUEUE)
 			__entry->flags |= 0x40;
 	),
 	TP_printk("packet ID: %08x, rate policy: %s %d|%d %d|%d %d|%d %d|%d -> %d attempt, Delays media/queue/total: %4dus/%4dus/%4dus",
diff --git a/drivers/staging/wfx/wfx.h b/drivers/staging/wfx/wfx.h
index 8b85bb1abb9c..73e216733ce4 100644
--- a/drivers/staging/wfx/wfx.h
+++ b/drivers/staging/wfx/wfx.h
@@ -21,10 +21,7 @@
 #include "main.h"
 #include "queue.h"
 #include "secure_link.h"
-#include "sta.h"
-#include "scan.h"
 #include "hif_tx.h"
-#include "hif_api_general.h"
 
 #define USEC_PER_TXOP 32 // see struct ieee80211_tx_queue_params
 #define USEC_PER_TU 1024
@@ -45,21 +42,24 @@ struct wfx_dev {
 	struct hif_ind_startup	hw_caps;
 	struct wfx_hif		hif;
 	struct sl_context	sl;
-	int			chip_frozen;
+	struct delayed_work	cooling_timeout_work;
+	bool			poll_irq;
+	bool			chip_frozen;
 	struct mutex		conf_mutex;
 
 	struct wfx_hif_cmd	hif_cmd;
 	struct wfx_queue	tx_queue[4];
-	struct wfx_queue_stats	tx_queue_stats;
-	int			tx_burst_idx;
+	struct sk_buff_head	tx_pending;
+	wait_queue_head_t	tx_dequeue;
 	atomic_t		tx_lock;
 
 	atomic_t		packet_id;
 	u32			key_map;
-	struct hif_req_add_key	keys[MAX_KEY_ENTRIES];
 
 	struct hif_rx_stats	rx_stats;
 	struct mutex		rx_stats_lock;
+	struct hif_tx_power_loop_info tx_power_loop_info;
+	struct mutex		tx_power_loop_info_lock;
 };
 
 struct wfx_vif {
@@ -67,42 +67,23 @@ struct wfx_vif {
 	struct ieee80211_vif	*vif;
 	struct ieee80211_channel *channel;
 	int			id;
-	enum wfx_state		state;
-
-	int			bss_loss_state;
-	u32			bss_loss_confirm_id;
-	struct mutex		bss_loss_lock;
-	struct delayed_work	bss_loss_work;
 
 	u32			link_id_map;
 
 	bool			after_dtim_tx_allowed;
-	struct wfx_grp_addr_table mcast_filter;
+	bool			join_in_progress;
 
-	s8			wep_default_key_id;
-	struct sk_buff		*wep_pending_skb;
-	struct work_struct	wep_key_work;
+	struct delayed_work	beacon_loss_work;
 
 	struct tx_policy_cache	tx_policy_cache;
 	struct work_struct	tx_policy_upload_work;
 
-	u32			sta_asleep_mask;
-	spinlock_t		ps_state_lock;
 	struct work_struct	update_tim_work;
 
-	int			beacon_int;
-	bool			filter_bssid;
-	bool			fwd_probe_req;
-	bool			disable_beacon_filter;
-	struct work_struct	update_filtering_work;
+	int			filter_mcast_count;
+	u8			filter_mcast_addr[8][ETH_ALEN];
 
 	unsigned long		uapsd_mask;
-	struct ieee80211_tx_queue_params edca_params[IEEE80211_NUM_ACS];
-	struct hif_req_set_bss_params bss_params;
-	struct work_struct	bss_params_work;
-
-	int			join_complete_status;
-	struct work_struct	unjoin_work;
 
 	/* avoid some operations in parallel with scan */
 	struct mutex		scan_lock;
@@ -111,11 +92,9 @@ struct wfx_vif {
 	bool			scan_abort;
 	struct ieee80211_scan_request *scan_req;
 
+	bool			bss_not_support_ps_poll;
+	struct work_struct	update_pm_work;
 	struct completion	set_pm_mode_complete;
-
-	struct list_head	event_queue;
-	spinlock_t		event_queue_lock;
-	struct work_struct	event_handler_work;
 };
 
 static inline struct wfx_vif *wdev_to_wvif(struct wfx_dev *wdev, int vif_id)
diff --git a/drivers/staging/wilc1000/hif.c b/drivers/staging/wilc1000/hif.c
index 6c7de2f8d3f2..d025a3093015 100644
--- a/drivers/staging/wilc1000/hif.c
+++ b/drivers/staging/wilc1000/hif.c
@@ -11,6 +11,8 @@
 
 #define WILC_FALSE_FRMWR_CHANNEL		100
 
+#define WILC_SCAN_WID_LIST_SIZE		6
+
 struct wilc_rcvd_mac_info {
 	u8 status;
 };
@@ -151,7 +153,7 @@ int wilc_scan(struct wilc_vif *vif, u8 scan_source, u8 scan_type,
 	      void *user_arg, struct cfg80211_scan_request *request)
 {
 	int result = 0;
-	struct wid wid_list[5];
+	struct wid wid_list[WILC_SCAN_WID_LIST_SIZE];
 	u32 index = 0;
 	u32 i, scan_timeout;
 	u8 *buffer;
author	Linus Torvalds <torvalds@linux-foundation.org>	2020-06-07 10:45:08 -0700
committer	Linus Torvalds <torvalds@linux-foundation.org>	2020-06-07 10:45:08 -0700
commit	80ef846e9909f22ccdc2a4a6d931266cecce8b2c (patch)
tree	0dd29cff6e6b4ee38452fb73ad292203c3ddeabf /drivers
parent	081096d98bb23946f16215357b141c5616b234bf (diff)
parent	77f55d1305c11fb729b88f2c3f7881ba0831fa6f (diff)