153 files changed, 669 insertions, 4971 deletions

diff --git a/MAINTAINERS b/MAINTAINERS
index 587f5fea8d6e..a15891e21ff9 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -4910,7 +4910,9 @@ S:	Maintained
 T:	git https://github.com/broadcom/stblinux.git
 F:	Documentation/devicetree/bindings/pci/brcm,stb-pcie.yaml
 F:	drivers/pci/controller/pcie-brcmstb.c
+F:	drivers/platform/raspberrypi/vchiq-*
 F:	drivers/staging/vc04_services
+F:	include/linux/raspberrypi/vchiq*
 N:	bcm2711
 N:	bcm2712
 N:	bcm283*
@@ -10727,7 +10729,9 @@ F:	drivers/platform/x86/gpd-pocket-fan.c
 GPIB DRIVERS
 M:	Dave Penkler <dpenkler@gmail.com>
 S:	Maintained
-F:	drivers/staging/gpib/
+F:	drivers/gpib/
+F:	include/uapi/linux/gpib.h
+F:	include/uapi/linux/gpib_ioctl.h
 
 GPIO ACPI SUPPORT
 M:	Mika Westerberg <westeri@kernel.org>
diff --git a/drivers/Kconfig b/drivers/Kconfig
index 3054b50a2f4c..c0f1fb893ec0 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -161,6 +161,8 @@ source "drivers/greybus/Kconfig"
 
 source "drivers/comedi/Kconfig"
 
+source "drivers/gpib/Kconfig"
+
 source "drivers/staging/Kconfig"
 
 source "drivers/platform/Kconfig"
diff --git a/drivers/Makefile b/drivers/Makefile
index b9f70e01f269..ccc05f1eae3e 100644
--- a/drivers/Makefile
+++ b/drivers/Makefile
@@ -150,6 +150,7 @@ obj-$(CONFIG_VHOST_IOTLB)	+= vhost/
 obj-$(CONFIG_VHOST)		+= vhost/
 obj-$(CONFIG_GREYBUS)		+= greybus/
 obj-$(CONFIG_COMEDI)		+= comedi/
+obj-$(CONFIG_GPIB)		+= gpib/
 obj-$(CONFIG_STAGING)		+= staging/
 obj-y				+= platform/
 
diff --git a/drivers/staging/gpib/Kconfig b/drivers/gpib/Kconfig
index aa01538d5beb..eeb50956ce85 100644
--- a/drivers/staging/gpib/Kconfig
+++ b/drivers/gpib/Kconfig
@@ -1,10 +1,10 @@
 # SPDX-License-Identifier: GPL-2.0
 menuconfig GPIB
-	tristate "Linux GPIB drivers"
+	tristate "GPIB drivers"
 	help
-	  Enable support for GPIB cards and dongles for Linux.  GPIB
-	  is the General Purpose Interface Bus which conforms to the
-	  IEEE488 standard.
+	  Enable support for GPIB cards and dongles.  GPIB is the
+	  General Purpose Interface Bus which conforms to the IEEE488
+	  standard.
 
 	  This set of drivers can be used with the corresponding user
 	  space library that can be found on Sourceforge under linux-gpib.
diff --git a/drivers/staging/gpib/Makefile b/drivers/gpib/Makefile
index d0e88f5c0844..2d44fed2a743 100644
--- a/drivers/staging/gpib/Makefile
+++ b/drivers/gpib/Makefile
@@ -1,5 +1,5 @@
 
-subdir-ccflags-y += -I$(src)/include -I$(src)/uapi
+subdir-ccflags-y += -I$(src)/include
 
 obj-$(CONFIG_GPIB_AGILENT_82350B) += agilent_82350b/
 obj-$(CONFIG_GPIB_AGILENT_82357A) += agilent_82357a/
diff --git a/drivers/staging/gpib/TODO b/drivers/gpib/TODO
index ab41a7f9ca5b..ac07dd90b4ef 100644
--- a/drivers/staging/gpib/TODO
+++ b/drivers/gpib/TODO
@@ -4,20 +4,6 @@ TODO:
     CHECK:ALLOC_SIZEOF_STRUCT: Prefer kmalloc(sizeof(*board->private_data)...) over kmalloc(sizeof(struct xxx_priv)...)
     ./gpio/gpib_bitbang.c:50: ERROR:COMPLEX_MACRO: Macros with complex values should be enclosed in parenthese
   This warning will be addressed later:  WARNING:UNDOCUMENTED_DT_STRING: DT compatible string
-- tidy-up comments:
-  - there are some "//comments" and "// comments" scattered around
-  - sometimes they are misaligned
-  - sometimes "// comments" are interleaved with "/* comments */"
-  - multiline comments should start with initial almost-blank line:
-    /*
-     * Good
-     * multiline
-     * comment
-     */
-    /* Bad
-     * multiline
-     * comment
-     */
 - resolve XXX notes where possible
 - fix FIXME notes
 - clean-up commented-out code
diff --git a/drivers/staging/gpib/agilent_82350b/Makefile b/drivers/gpib/agilent_82350b/Makefile
index f24e1e713a63..f24e1e713a63 100644
--- a/drivers/staging/gpib/agilent_82350b/Makefile
+++ b/drivers/gpib/agilent_82350b/Makefile
diff --git a/drivers/staging/gpib/agilent_82350b/agilent_82350b.c b/drivers/gpib/agilent_82350b/agilent_82350b.c
index 01a5bb43cd2d..01a5bb43cd2d 100644
--- a/drivers/staging/gpib/agilent_82350b/agilent_82350b.c
+++ b/drivers/gpib/agilent_82350b/agilent_82350b.c
diff --git a/drivers/staging/gpib/agilent_82350b/agilent_82350b.h b/drivers/gpib/agilent_82350b/agilent_82350b.h
index ef841957297f..ef841957297f 100644
--- a/drivers/staging/gpib/agilent_82350b/agilent_82350b.h
+++ b/drivers/gpib/agilent_82350b/agilent_82350b.h
diff --git a/drivers/staging/gpib/agilent_82357a/Makefile b/drivers/gpib/agilent_82357a/Makefile
index 81a55c257a6e..81a55c257a6e 100644
--- a/drivers/staging/gpib/agilent_82357a/Makefile
+++ b/drivers/gpib/agilent_82357a/Makefile
diff --git a/drivers/staging/gpib/agilent_82357a/agilent_82357a.c b/drivers/gpib/agilent_82357a/agilent_82357a.c
index 77c8e549b208..77c8e549b208 100644
--- a/drivers/staging/gpib/agilent_82357a/agilent_82357a.c
+++ b/drivers/gpib/agilent_82357a/agilent_82357a.c
diff --git a/drivers/staging/gpib/agilent_82357a/agilent_82357a.h b/drivers/gpib/agilent_82357a/agilent_82357a.h
index 33ac558e5552..33ac558e5552 100644
--- a/drivers/staging/gpib/agilent_82357a/agilent_82357a.h
+++ b/drivers/gpib/agilent_82357a/agilent_82357a.h
diff --git a/drivers/staging/gpib/cb7210/Makefile b/drivers/gpib/cb7210/Makefile
index d239ae80b415..d239ae80b415 100644
--- a/drivers/staging/gpib/cb7210/Makefile
+++ b/drivers/gpib/cb7210/Makefile
diff --git a/drivers/staging/gpib/cb7210/cb7210.c b/drivers/gpib/cb7210/cb7210.c
index 3e2397898a9b..24c61b151071 100644
--- a/drivers/staging/gpib/cb7210/cb7210.c
+++ b/drivers/gpib/cb7210/cb7210.c
@@ -1290,26 +1290,14 @@ static void cb_gpib_release(struct pcmcia_device *link)
 
 static int cb_gpib_suspend(struct pcmcia_device *link)
 {
-	//struct local_info *info = link->priv;
-	//struct struct gpib_board *dev = info->dev;
-
 	if (link->open)
 		dev_warn(&link->dev, "Device still open\n");
-	//netif_device_detach(dev);
 
 	return 0;
 }
 
 static int cb_gpib_resume(struct pcmcia_device *link)
 {
-	//struct local_info *info = link->priv;
-	//struct struct gpib_board *dev = info->dev;
-
-	/*if (link->open) {
-	 *	ni_gpib_probe(dev);	/ really?
-	 *	//netif_device_attach(dev);
-	 *
-	 */
 	return cb_gpib_config(link);
 }
 
diff --git a/drivers/staging/gpib/cb7210/cb7210.h b/drivers/gpib/cb7210/cb7210.h
index ddc841ff87ae..ddc841ff87ae 100644
--- a/drivers/staging/gpib/cb7210/cb7210.h
+++ b/drivers/gpib/cb7210/cb7210.h
diff --git a/drivers/staging/gpib/cec/Makefile b/drivers/gpib/cec/Makefile
index b7141e23d4e0..b7141e23d4e0 100644
--- a/drivers/staging/gpib/cec/Makefile
+++ b/drivers/gpib/cec/Makefile
diff --git a/drivers/staging/gpib/cec/cec.h b/drivers/gpib/cec/cec.h
index 3ce2869c7429..3ce2869c7429 100644
--- a/drivers/staging/gpib/cec/cec.h
+++ b/drivers/gpib/cec/cec.h
diff --git a/drivers/staging/gpib/cec/cec_gpib.c b/drivers/gpib/cec/cec_gpib.c
index dbf9b95baabc..dbf9b95baabc 100644
--- a/drivers/staging/gpib/cec/cec_gpib.c
+++ b/drivers/gpib/cec/cec_gpib.c
diff --git a/drivers/staging/gpib/common/Makefile b/drivers/gpib/common/Makefile
index 460586edb574..460586edb574 100644
--- a/drivers/staging/gpib/common/Makefile
+++ b/drivers/gpib/common/Makefile
diff --git a/drivers/staging/gpib/common/gpib_os.c b/drivers/gpib/common/gpib_os.c
index 9dbbac8b8436..9dbbac8b8436 100644
--- a/drivers/staging/gpib/common/gpib_os.c
+++ b/drivers/gpib/common/gpib_os.c
diff --git a/drivers/staging/gpib/common/iblib.c b/drivers/gpib/common/iblib.c
index 7cbb6a467177..7cbb6a467177 100644
--- a/drivers/staging/gpib/common/iblib.c
+++ b/drivers/gpib/common/iblib.c
diff --git a/drivers/staging/gpib/common/ibsys.h b/drivers/gpib/common/ibsys.h
index e5a148f513a8..e5a148f513a8 100644
--- a/drivers/staging/gpib/common/ibsys.h
+++ b/drivers/gpib/common/ibsys.h
diff --git a/drivers/staging/gpib/eastwood/Makefile b/drivers/gpib/eastwood/Makefile
index 384825195f77..384825195f77 100644
--- a/drivers/staging/gpib/eastwood/Makefile
+++ b/drivers/gpib/eastwood/Makefile
diff --git a/drivers/staging/gpib/eastwood/fluke_gpib.c b/drivers/gpib/eastwood/fluke_gpib.c
index 3ae848e3f738..3ae848e3f738 100644
--- a/drivers/staging/gpib/eastwood/fluke_gpib.c
+++ b/drivers/gpib/eastwood/fluke_gpib.c
diff --git a/drivers/staging/gpib/eastwood/fluke_gpib.h b/drivers/gpib/eastwood/fluke_gpib.h
index 493c200d0bbf..493c200d0bbf 100644
--- a/drivers/staging/gpib/eastwood/fluke_gpib.h
+++ b/drivers/gpib/eastwood/fluke_gpib.h
diff --git a/drivers/staging/gpib/fmh_gpib/Makefile b/drivers/gpib/fmh_gpib/Makefile
index cc4d9e7cd5cd..cc4d9e7cd5cd 100644
--- a/drivers/staging/gpib/fmh_gpib/Makefile
+++ b/drivers/gpib/fmh_gpib/Makefile
diff --git a/drivers/staging/gpib/fmh_gpib/fmh_gpib.c b/drivers/gpib/fmh_gpib/fmh_gpib.c
index f7bfb4a8e553..f7bfb4a8e553 100644
--- a/drivers/staging/gpib/fmh_gpib/fmh_gpib.c
+++ b/drivers/gpib/fmh_gpib/fmh_gpib.c
diff --git a/drivers/staging/gpib/fmh_gpib/fmh_gpib.h b/drivers/gpib/fmh_gpib/fmh_gpib.h
index e7602d7e1401..e7602d7e1401 100644
--- a/drivers/staging/gpib/fmh_gpib/fmh_gpib.h
+++ b/drivers/gpib/fmh_gpib/fmh_gpib.h
diff --git a/drivers/staging/gpib/gpio/Makefile b/drivers/gpib/gpio/Makefile
index 00ea52abdda7..00ea52abdda7 100644
--- a/drivers/staging/gpib/gpio/Makefile
+++ b/drivers/gpib/gpio/Makefile
diff --git a/drivers/staging/gpib/gpio/gpib_bitbang.c b/drivers/gpib/gpio/gpib_bitbang.c
index 374cd61355e9..374cd61355e9 100644
--- a/drivers/staging/gpib/gpio/gpib_bitbang.c
+++ b/drivers/gpib/gpio/gpib_bitbang.c
diff --git a/drivers/staging/gpib/hp_82335/Makefile b/drivers/gpib/hp_82335/Makefile
index 305ce44ee48a..305ce44ee48a 100644
--- a/drivers/staging/gpib/hp_82335/Makefile
+++ b/drivers/gpib/hp_82335/Makefile
diff --git a/drivers/staging/gpib/hp_82335/hp82335.c b/drivers/gpib/hp_82335/hp82335.c
index d0e47ef77c87..d0e47ef77c87 100644
--- a/drivers/staging/gpib/hp_82335/hp82335.c
+++ b/drivers/gpib/hp_82335/hp82335.c
diff --git a/drivers/staging/gpib/hp_82335/hp82335.h b/drivers/gpib/hp_82335/hp82335.h
index 0c252a712ec9..0c252a712ec9 100644
--- a/drivers/staging/gpib/hp_82335/hp82335.h
+++ b/drivers/gpib/hp_82335/hp82335.h
diff --git a/drivers/staging/gpib/hp_82341/Makefile b/drivers/gpib/hp_82341/Makefile
index 21367310a17e..21367310a17e 100644
--- a/drivers/staging/gpib/hp_82341/Makefile
+++ b/drivers/gpib/hp_82341/Makefile
diff --git a/drivers/staging/gpib/hp_82341/hp_82341.c b/drivers/gpib/hp_82341/hp_82341.c
index 1a2ad0560e14..1a2ad0560e14 100644
--- a/drivers/staging/gpib/hp_82341/hp_82341.c
+++ b/drivers/gpib/hp_82341/hp_82341.c
diff --git a/drivers/staging/gpib/hp_82341/hp_82341.h b/drivers/gpib/hp_82341/hp_82341.h
index 859ef2899acb..859ef2899acb 100644
--- a/drivers/staging/gpib/hp_82341/hp_82341.h
+++ b/drivers/gpib/hp_82341/hp_82341.h
diff --git a/drivers/staging/gpib/include/amcc5920.h b/drivers/gpib/include/amcc5920.h
index 7a88bd282feb..7a88bd282feb 100644
--- a/drivers/staging/gpib/include/amcc5920.h
+++ b/drivers/gpib/include/amcc5920.h
diff --git a/drivers/staging/gpib/include/amccs5933.h b/drivers/gpib/include/amccs5933.h
index d7f63c795096..d7f63c795096 100644
--- a/drivers/staging/gpib/include/amccs5933.h
+++ b/drivers/gpib/include/amccs5933.h
diff --git a/drivers/staging/gpib/include/gpibP.h b/drivers/gpib/include/gpibP.h
index 1b27f37e0ba0..e3938ada3e0d 100644
--- a/drivers/staging/gpib/include/gpibP.h
+++ b/drivers/gpib/include/gpibP.h
@@ -12,8 +12,8 @@
 #include "gpib_types.h"
 #include "gpib_proto.h"
 #include "gpib_cmd.h"
-#include "gpib.h"
-#include "gpib_ioctl.h"
+#include <linux/gpib.h>
+#include <linux/gpib_ioctl.h>
 
 #include <linux/fs.h>
 #include <linux/interrupt.h>
diff --git a/drivers/staging/gpib/include/gpib_cmd.h b/drivers/gpib/include/gpib_cmd.h
index 9e96a3bfa22d..9e96a3bfa22d 100644
--- a/drivers/staging/gpib/include/gpib_cmd.h
+++ b/drivers/gpib/include/gpib_cmd.h
diff --git a/drivers/staging/gpib/include/gpib_pci_ids.h b/drivers/gpib/include/gpib_pci_ids.h
index 52dcab07a7d1..52dcab07a7d1 100644
--- a/drivers/staging/gpib/include/gpib_pci_ids.h
+++ b/drivers/gpib/include/gpib_pci_ids.h
diff --git a/drivers/staging/gpib/include/gpib_proto.h b/drivers/gpib/include/gpib_proto.h
index 42e736e3b7cd..42e736e3b7cd 100644
--- a/drivers/staging/gpib/include/gpib_proto.h
+++ b/drivers/gpib/include/gpib_proto.h
diff --git a/drivers/staging/gpib/include/gpib_state_machines.h b/drivers/gpib/include/gpib_state_machines.h
index 7488c00f191e..7488c00f191e 100644
--- a/drivers/staging/gpib/include/gpib_state_machines.h
+++ b/drivers/gpib/include/gpib_state_machines.h
diff --git a/drivers/staging/gpib/include/gpib_types.h b/drivers/gpib/include/gpib_types.h
index 998abb379749..5a0978ae27e7 100644
--- a/drivers/staging/gpib/include/gpib_types.h
+++ b/drivers/gpib/include/gpib_types.h
@@ -8,7 +8,7 @@
 #define _GPIB_TYPES_H
 
 #ifdef __KERNEL__
-#include "gpib.h"
+#include <linux/gpib.h>
 #include <linux/atomic.h>
 #include <linux/device.h>
 #include <linux/mutex.h>
diff --git a/drivers/staging/gpib/include/nec7210.h b/drivers/gpib/include/nec7210.h
index 9835aa5ef4ff..9835aa5ef4ff 100644
--- a/drivers/staging/gpib/include/nec7210.h
+++ b/drivers/gpib/include/nec7210.h
diff --git a/drivers/staging/gpib/include/nec7210_registers.h b/drivers/gpib/include/nec7210_registers.h
index 067983d7a07f..067983d7a07f 100644
--- a/drivers/staging/gpib/include/nec7210_registers.h
+++ b/drivers/gpib/include/nec7210_registers.h
diff --git a/drivers/staging/gpib/include/plx9050.h b/drivers/gpib/include/plx9050.h
index c911b285a0ca..c911b285a0ca 100644
--- a/drivers/staging/gpib/include/plx9050.h
+++ b/drivers/gpib/include/plx9050.h
diff --git a/drivers/staging/gpib/include/quancom_pci.h b/drivers/gpib/include/quancom_pci.h
index cdaf0d056be9..cdaf0d056be9 100644
--- a/drivers/staging/gpib/include/quancom_pci.h
+++ b/drivers/gpib/include/quancom_pci.h
diff --git a/drivers/staging/gpib/include/tms9914.h b/drivers/gpib/include/tms9914.h
index e66b75e0fda8..e66b75e0fda8 100644
--- a/drivers/staging/gpib/include/tms9914.h
+++ b/drivers/gpib/include/tms9914.h
diff --git a/drivers/staging/gpib/include/tnt4882_registers.h b/drivers/gpib/include/tnt4882_registers.h
index d54c4cc61168..d54c4cc61168 100644
--- a/drivers/staging/gpib/include/tnt4882_registers.h
+++ b/drivers/gpib/include/tnt4882_registers.h
diff --git a/drivers/staging/gpib/ines/Makefile b/drivers/gpib/ines/Makefile
index 88241f15ecea..88241f15ecea 100644
--- a/drivers/staging/gpib/ines/Makefile
+++ b/drivers/gpib/ines/Makefile
diff --git a/drivers/staging/gpib/ines/ines.h b/drivers/gpib/ines/ines.h
index 6ad57e9a1216..6ad57e9a1216 100644
--- a/drivers/staging/gpib/ines/ines.h
+++ b/drivers/gpib/ines/ines.h
diff --git a/drivers/staging/gpib/ines/ines_gpib.c b/drivers/gpib/ines/ines_gpib.c
index a3cf846fd0f9..a3cf846fd0f9 100644
--- a/drivers/staging/gpib/ines/ines_gpib.c
+++ b/drivers/gpib/ines/ines_gpib.c
diff --git a/drivers/staging/gpib/lpvo_usb_gpib/Makefile b/drivers/gpib/lpvo_usb_gpib/Makefile
index 360553488e6d..360553488e6d 100644
--- a/drivers/staging/gpib/lpvo_usb_gpib/Makefile
+++ b/drivers/gpib/lpvo_usb_gpib/Makefile
diff --git a/drivers/staging/gpib/lpvo_usb_gpib/lpvo_usb_gpib.c b/drivers/gpib/lpvo_usb_gpib/lpvo_usb_gpib.c
index dd68c4843490..dd68c4843490 100644
--- a/drivers/staging/gpib/lpvo_usb_gpib/lpvo_usb_gpib.c
+++ b/drivers/gpib/lpvo_usb_gpib/lpvo_usb_gpib.c
diff --git a/drivers/staging/gpib/nec7210/Makefile b/drivers/gpib/nec7210/Makefile
index 64330f2e89d1..64330f2e89d1 100644
--- a/drivers/staging/gpib/nec7210/Makefile
+++ b/drivers/gpib/nec7210/Makefile
diff --git a/drivers/staging/gpib/nec7210/board.h b/drivers/gpib/nec7210/board.h
index ac3fe38ade57..ac3fe38ade57 100644
--- a/drivers/staging/gpib/nec7210/board.h
+++ b/drivers/gpib/nec7210/board.h
diff --git a/drivers/staging/gpib/nec7210/nec7210.c b/drivers/gpib/nec7210/nec7210.c
index bbf39367f5e4..bbf39367f5e4 100644
--- a/drivers/staging/gpib/nec7210/nec7210.c
+++ b/drivers/gpib/nec7210/nec7210.c
diff --git a/drivers/staging/gpib/ni_usb/Makefile b/drivers/gpib/ni_usb/Makefile
index 469c5d16add3..469c5d16add3 100644
--- a/drivers/staging/gpib/ni_usb/Makefile
+++ b/drivers/gpib/ni_usb/Makefile
diff --git a/drivers/staging/gpib/ni_usb/ni_usb_gpib.c b/drivers/gpib/ni_usb/ni_usb_gpib.c
index 1f8412de9fa3..1f8412de9fa3 100644
--- a/drivers/staging/gpib/ni_usb/ni_usb_gpib.c
+++ b/drivers/gpib/ni_usb/ni_usb_gpib.c
diff --git a/drivers/staging/gpib/ni_usb/ni_usb_gpib.h b/drivers/gpib/ni_usb/ni_usb_gpib.h
index 688f5e08792f..688f5e08792f 100644
--- a/drivers/staging/gpib/ni_usb/ni_usb_gpib.h
+++ b/drivers/gpib/ni_usb/ni_usb_gpib.h
diff --git a/drivers/staging/gpib/pc2/Makefile b/drivers/gpib/pc2/Makefile
index 481ee4296e1b..481ee4296e1b 100644
--- a/drivers/staging/gpib/pc2/Makefile
+++ b/drivers/gpib/pc2/Makefile
diff --git a/drivers/staging/gpib/pc2/pc2_gpib.c b/drivers/gpib/pc2/pc2_gpib.c
index 9f3943d1df66..9f3943d1df66 100644
--- a/drivers/staging/gpib/pc2/pc2_gpib.c
+++ b/drivers/gpib/pc2/pc2_gpib.c
diff --git a/drivers/staging/gpib/tms9914/Makefile b/drivers/gpib/tms9914/Makefile
index 4705ab07f413..4705ab07f413 100644
--- a/drivers/staging/gpib/tms9914/Makefile
+++ b/drivers/gpib/tms9914/Makefile
diff --git a/drivers/staging/gpib/tms9914/tms9914.c b/drivers/gpib/tms9914/tms9914.c
index 0d11b80bb982..72a11596a35e 100644
--- a/drivers/staging/gpib/tms9914/tms9914.c
+++ b/drivers/gpib/tms9914/tms9914.c
@@ -535,7 +535,7 @@ int tms9914_read(struct gpib_board *board, struct tms9914_priv *priv, u8 *buffer
 		buffer += num_bytes;
 		length -= num_bytes;
 	}
-	// read last bytes if we havn't received an END yet
+	// read last bytes if we haven't received an END yet
 	if (*end == 0) {
 		// make sure we holdoff after last byte read
 		tms9914_set_holdoff_mode(priv, TMS9914_HOLDOFF_ALL);
diff --git a/drivers/staging/gpib/tnt4882/Makefile b/drivers/gpib/tnt4882/Makefile
index fa1687ad0d1b..fa1687ad0d1b 100644
--- a/drivers/staging/gpib/tnt4882/Makefile
+++ b/drivers/gpib/tnt4882/Makefile
diff --git a/drivers/staging/gpib/tnt4882/mite.c b/drivers/gpib/tnt4882/mite.c
index 847b96f411bd..847b96f411bd 100644
--- a/drivers/staging/gpib/tnt4882/mite.c
+++ b/drivers/gpib/tnt4882/mite.c
diff --git a/drivers/staging/gpib/tnt4882/mite.h b/drivers/gpib/tnt4882/mite.h
index a1fdba9672a0..a1fdba9672a0 100644
--- a/drivers/staging/gpib/tnt4882/mite.h
+++ b/drivers/gpib/tnt4882/mite.h
diff --git a/drivers/staging/gpib/tnt4882/tnt4882_gpib.c b/drivers/gpib/tnt4882/tnt4882_gpib.c
index c03a976b7380..c03a976b7380 100644
--- a/drivers/staging/gpib/tnt4882/tnt4882_gpib.c
+++ b/drivers/gpib/tnt4882/tnt4882_gpib.c
diff --git a/drivers/platform/Kconfig b/drivers/platform/Kconfig
index 960fd6a82450..324c69c63f76 100644
--- a/drivers/platform/Kconfig
+++ b/drivers/platform/Kconfig
@@ -18,3 +18,5 @@ source "drivers/platform/surface/Kconfig"
 source "drivers/platform/x86/Kconfig"
 
 source "drivers/platform/arm64/Kconfig"
+
+source "drivers/platform/raspberrypi/Kconfig"
diff --git a/drivers/platform/Makefile b/drivers/platform/Makefile
index 19ac54648586..b0935c602ada 100644
--- a/drivers/platform/Makefile
+++ b/drivers/platform/Makefile
@@ -13,3 +13,4 @@ obj-$(CONFIG_CHROME_PLATFORMS)	+= chrome/
 obj-$(CONFIG_CZNIC_PLATFORMS)	+= cznic/
 obj-$(CONFIG_SURFACE_PLATFORMS)	+= surface/
 obj-$(CONFIG_ARM64_PLATFORM_DEVICES)	+= arm64/
+obj-$(CONFIG_BCM2835_VCHIQ)	+= raspberrypi/
diff --git a/drivers/platform/raspberrypi/Kconfig b/drivers/platform/raspberrypi/Kconfig
new file mode 100644
index 000000000000..2c928440a47c
--- /dev/null
+++ b/drivers/platform/raspberrypi/Kconfig
@@ -0,0 +1,52 @@
+# SPDX-License-Identifier: GPL-2.0
+
+menuconfig BCM_VIDEOCORE
+	tristate "Broadcom VideoCore support"
+	depends on OF
+	depends on RASPBERRYPI_FIRMWARE || (COMPILE_TEST && !RASPBERRYPI_FIRMWARE)
+	default y
+	help
+		Support for Broadcom VideoCore services including
+		the BCM2835 family of products which is used
+		by the Raspberry PI.
+
+if BCM_VIDEOCORE
+
+config BCM2835_VCHIQ
+	tristate "BCM2835 VCHIQ"
+	depends on HAS_DMA
+	imply VCHIQ_CDEV
+	help
+		Broadcom BCM2835 and similar SoCs have a VPU called VideoCore.
+		This config enables the VCHIQ driver, which implements a
+		messaging interface between the kernel and the firmware running
+		on VideoCore. Other drivers use this interface to communicate to
+		the VPU. More specifically, the VCHIQ driver is used by
+		audio/video and camera drivers as well as for implementing MMAL
+		API, which is in turn used by several multimedia services on the
+		BCM2835 family of SoCs.
+
+		Defaults to Y when the Broadcom Videocore services are included
+		in the build, N otherwise.
+
+if BCM2835_VCHIQ
+
+config VCHIQ_CDEV
+	bool "VCHIQ Character Driver"
+	help
+		Enable the creation of VCHIQ character driver. The cdev exposes
+		ioctls used by userspace libraries and testing tools to interact
+		with VideoCore, via the VCHIQ core driver (Check BCM2835_VCHIQ
+		for more info).
+
+		This can be set to 'N' if the VideoCore communication is not
+		needed by userspace but only by other kernel modules
+		(like bcm2835-audio).
+
+		If not sure, set this to 'Y'.
+
+endif
+
+source "drivers/platform/raspberrypi/vchiq-mmal/Kconfig"
+
+endif
diff --git a/drivers/platform/raspberrypi/Makefile b/drivers/platform/raspberrypi/Makefile
new file mode 100644
index 000000000000..2a7c9511e5d8
--- /dev/null
+++ b/drivers/platform/raspberrypi/Makefile
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_BCM2835_VCHIQ)	+= vchiq.o
+
+vchiq-objs := \
+   vchiq-interface/vchiq_core.o  \
+   vchiq-interface/vchiq_arm.o \
+   vchiq-interface/vchiq_bus.o \
+   vchiq-interface/vchiq_debugfs.o \
+
+ifdef CONFIG_VCHIQ_CDEV
+vchiq-objs += vchiq-interface/vchiq_dev.o
+endif
+
+obj-$(CONFIG_BCM2835_VCHIQ_MMAL)	+= vchiq-mmal/
diff --git a/drivers/staging/vc04_services/interface/TESTING b/drivers/platform/raspberrypi/vchiq-interface/TESTING
index c98f688b07e0..c98f688b07e0 100644
--- a/drivers/staging/vc04_services/interface/TESTING
+++ b/drivers/platform/raspberrypi/vchiq-interface/TESTING
diff --git a/drivers/platform/raspberrypi/vchiq-interface/TODO b/drivers/platform/raspberrypi/vchiq-interface/TODO
new file mode 100644
index 000000000000..2357dae413f1
--- /dev/null
+++ b/drivers/platform/raspberrypi/vchiq-interface/TODO
@@ -0,0 +1,4 @@
+* Documentation
+
+A short top-down description of this driver's architecture (function of
+kthreads, userspace, limitations) could be very helpful for reviewers.
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c b/drivers/platform/raspberrypi/vchiq-interface/vchiq_arm.c
index 721b15b7e13b..6a7b96d3dae6 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_arm.c
@@ -30,11 +30,12 @@
 #include <linux/uaccess.h>
 #include <soc/bcm2835/raspberrypi-firmware.h>
 
-#include "vchiq_core.h"
+#include <linux/raspberrypi/vchiq_core.h>
+#include <linux/raspberrypi/vchiq_arm.h>
+#include <linux/raspberrypi/vchiq_bus.h>
+#include <linux/raspberrypi/vchiq_debugfs.h>
+
 #include "vchiq_ioctl.h"
-#include "vchiq_arm.h"
-#include "vchiq_bus.h"
-#include "vchiq_debugfs.h"
 
 #define DEVICE_NAME "vchiq"
 
@@ -62,7 +63,6 @@
  * the interface.
  */
 static struct vchiq_device *bcm2835_audio;
-static struct vchiq_device *bcm2835_camera;
 
 static const struct vchiq_platform_info bcm2835_info = {
 	.cache_line_size = 32,
@@ -73,7 +73,13 @@ static const struct vchiq_platform_info bcm2836_info = {
 };
 
 struct vchiq_arm_state {
-	/* Keepalive-related data */
+	/*
+	 * Keepalive-related data
+	 *
+	 * The keepalive mechanism was retro-fitted to VCHIQ to allow active
+	 * services to prevent the system from suspending.
+	 * This feature is not used on Raspberry Pi devices.
+	 */
 	struct task_struct *ka_thread;
 	struct completion ka_evt;
 	atomic_t ka_use_count;
@@ -1416,7 +1422,6 @@ static int vchiq_probe(struct platform_device *pdev)
 	vchiq_debugfs_init(&mgmt->state);
 
 	bcm2835_audio = vchiq_device_register(&pdev->dev, "bcm2835-audio");
-	bcm2835_camera = vchiq_device_register(&pdev->dev, "bcm2835-camera");
 
 	return 0;
 }
@@ -1426,7 +1431,6 @@ static void vchiq_remove(struct platform_device *pdev)
 	struct vchiq_drv_mgmt *mgmt = dev_get_drvdata(&pdev->dev);
 
 	vchiq_device_unregister(bcm2835_audio);
-	vchiq_device_unregister(bcm2835_camera);
 	vchiq_debugfs_deinit();
 	vchiq_deregister_chrdev();
 	vchiq_platform_uninit(mgmt);
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_bus.c b/drivers/platform/raspberrypi/vchiq-interface/vchiq_bus.c
index 41ece91ab88a..f50e637d505c 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_bus.c
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_bus.c
@@ -11,8 +11,8 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 
-#include "vchiq_arm.h"
-#include "vchiq_bus.h"
+#include <linux/raspberrypi/vchiq_arm.h>
+#include <linux/raspberrypi/vchiq_bus.h>
 
 static int vchiq_bus_type_match(struct device *dev, const struct device_driver *drv)
 {
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c b/drivers/platform/raspberrypi/vchiq-interface/vchiq_core.c
index e2cac0898b8f..83de27cfd469 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_core.c
@@ -15,8 +15,8 @@
 #include <linux/rcupdate.h>
 #include <linux/sched/signal.h>
 
-#include "vchiq_arm.h"
-#include "vchiq_core.h"
+#include <linux/raspberrypi/vchiq_arm.h>
+#include <linux/raspberrypi/vchiq_core.h>
 
 #define VCHIQ_SLOT_HANDLER_STACK 8192
 
@@ -4001,10 +4001,7 @@ void vchiq_log_dump_mem(struct device *dev, const char *label, u32 addr,
 		}
 		*s++ = '\0';
 
-		if (label && (*label != '\0'))
-			dev_dbg(dev, "core: %s: %08x: %s\n", label, addr, line_buf);
-		else
-			dev_dbg(dev, "core: %s: %08x: %s\n", label, addr, line_buf);
+		dev_dbg(dev, "core: %s: %08x: %s\n", label, addr, line_buf);
 
 		addr += 16;
 		mem += 16;
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c b/drivers/platform/raspberrypi/vchiq-interface/vchiq_debugfs.c
index d5f7f61c5626..c82326a9b6d9 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.c
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_debugfs.c
@@ -5,9 +5,9 @@
  */
 
 #include <linux/debugfs.h>
-#include "vchiq_core.h"
-#include "vchiq_arm.h"
-#include "vchiq_debugfs.h"
+#include <linux/raspberrypi/vchiq_core.h>
+#include <linux/raspberrypi/vchiq_arm.h>
+#include <linux/raspberrypi/vchiq_debugfs.h>
 
 #ifdef CONFIG_DEBUG_FS
 
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c b/drivers/platform/raspberrypi/vchiq-interface/vchiq_dev.c
index 3b20ba5c7362..0f3dde2657d6 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_dev.c
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_dev.c
@@ -11,10 +11,11 @@
 #include <linux/compat.h>
 #include <linux/miscdevice.h>
 
-#include "vchiq_core.h"
+#include <linux/raspberrypi/vchiq_core.h>
+#include <linux/raspberrypi/vchiq_arm.h>
+#include <linux/raspberrypi/vchiq_debugfs.h>
+
 #include "vchiq_ioctl.h"
-#include "vchiq_arm.h"
-#include "vchiq_debugfs.h"
 
 static const char *const ioctl_names[] = {
 	"CONNECT",
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h b/drivers/platform/raspberrypi/vchiq-interface/vchiq_ioctl.h
index afb71a83cfe7..d0c759f6d8ea 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_ioctl.h
+++ b/drivers/platform/raspberrypi/vchiq-interface/vchiq_ioctl.h
@@ -5,8 +5,7 @@
 #define VCHIQ_IOCTLS_H
 
 #include <linux/ioctl.h>
-
-#include "../../include/linux/raspberrypi/vchiq.h"
+#include <linux/raspberrypi/vchiq.h>
 
 #define VCHIQ_IOC_MAGIC 0xc4
 #define VCHIQ_INVALID_HANDLE (~0)
diff --git a/drivers/staging/vc04_services/vchiq-mmal/Kconfig b/drivers/platform/raspberrypi/vchiq-mmal/Kconfig
index c99525a0bb45..c99525a0bb45 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/Kconfig
+++ b/drivers/platform/raspberrypi/vchiq-mmal/Kconfig
diff --git a/drivers/staging/vc04_services/vchiq-mmal/Makefile b/drivers/platform/raspberrypi/vchiq-mmal/Makefile
index 6937f6534c26..6937f6534c26 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/Makefile
+++ b/drivers/platform/raspberrypi/vchiq-mmal/Makefile
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-common.h
index b33129403a30..b33129403a30 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-common.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-common.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-encodings.h
index e15ae7b24f73..e15ae7b24f73 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-encodings.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-encodings.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-common.h
index 492d4c5dca08..492d4c5dca08 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-common.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-common.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-format.h
index 5569876d8c7d..5569876d8c7d 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-format.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-format.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-port.h
index 6ee4c1ed7f19..6ee4c1ed7f19 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg-port.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg-port.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg.h
index 1889494425eb..1889494425eb 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-msg.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-msg.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-parameters.h
index a0cdd28101f2..a0cdd28101f2 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-parameters.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-parameters.h
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c b/drivers/platform/raspberrypi/vchiq-mmal/mmal-vchiq.c
index c2b5a37915f2..cd073ed3ea2d 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-vchiq.c
@@ -22,11 +22,12 @@
 #include <linux/mm.h>
 #include <linux/slab.h>
 #include <linux/completion.h>
+#include <linux/raspberrypi/vchiq.h>
 #include <linux/vmalloc.h>
 #include <media/videobuf2-vmalloc.h>
 
-#include "../include/linux/raspberrypi/vchiq.h"
-#include "../interface/vchiq_arm/vchiq_arm.h"
+#include <linux/raspberrypi/vchiq_arm.h>
+
 #include "mmal-common.h"
 #include "mmal-vchiq.h"
 #include "mmal-msg.h"
diff --git a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h b/drivers/platform/raspberrypi/vchiq-mmal/mmal-vchiq.h
index 8c3959f6f97f..8c3959f6f97f 100644
--- a/drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.h
+++ b/drivers/platform/raspberrypi/vchiq-mmal/mmal-vchiq.h
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 075e775d3868..2f92cd698bef 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -48,6 +48,4 @@ source "drivers/staging/axis-fifo/Kconfig"
 
 source "drivers/staging/vme_user/Kconfig"
 
-source "drivers/staging/gpib/Kconfig"
-
 endif # STAGING
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index e681e403509c..f5b8876aa536 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -13,4 +13,3 @@ obj-$(CONFIG_MOST)		+= most/
 obj-$(CONFIG_GREYBUS)		+= greybus/
 obj-$(CONFIG_BCM2835_VCHIQ)	+= vc04_services/
 obj-$(CONFIG_XIL_AXIS_FIFO)	+= axis-fifo/
-obj-$(CONFIG_GPIB)	 	+= gpib/
diff --git a/drivers/staging/axis-fifo/axis-fifo.c b/drivers/staging/axis-fifo/axis-fifo.c
index 811bfdc578d8..509d620d6ce7 100644
--- a/drivers/staging/axis-fifo/axis-fifo.c
+++ b/drivers/staging/axis-fifo/axis-fifo.c
@@ -88,16 +88,8 @@
 #define XLLF_INT_TC_MASK          0x08000000 /* Transmit complete */
 #define XLLF_INT_RC_MASK          0x04000000 /* Receive complete */
 #define XLLF_INT_TSE_MASK         0x02000000 /* Transmit length mismatch */
-#define XLLF_INT_TRC_MASK         0x01000000 /* Transmit reset complete */
-#define XLLF_INT_RRC_MASK         0x00800000 /* Receive reset complete */
-#define XLLF_INT_TFPF_MASK        0x00400000 /* Tx FIFO Programmable Full */
-#define XLLF_INT_TFPE_MASK        0x00200000 /* Tx FIFO Programmable Empty */
-#define XLLF_INT_RFPF_MASK        0x00100000 /* Rx FIFO Programmable Full */
-#define XLLF_INT_RFPE_MASK        0x00080000 /* Rx FIFO Programmable Empty */
-#define XLLF_INT_ALL_MASK         0xfff80000 /* All the ints */
-#define XLLF_INT_ERROR_MASK       0xf2000000 /* Error status ints */
-#define XLLF_INT_RXERROR_MASK     0xe0000000 /* Receive Error status ints */
-#define XLLF_INT_TXERROR_MASK     0x12000000 /* Transmit Error status ints */
+
+#define XLLF_INT_CLEAR_ALL	GENMASK(31, 0)
 
 /* ----------------------------
  *           globals
@@ -125,7 +117,6 @@ MODULE_PARM_DESC(write_timeout, "ms to wait before blocking write() timing out;
 
 struct axis_fifo {
 	int id;
-	int irq; /* interrupt */
 	void __iomem *base_addr; /* kernel space memory */
 
 	unsigned int rx_fifo_depth; /* max words in the receive fifo */
@@ -137,8 +128,6 @@ struct axis_fifo {
 	struct mutex read_lock; /* lock for reading */
 	wait_queue_head_t write_queue; /* wait queue for asynchronos write */
 	struct mutex write_lock; /* lock for writing */
-	unsigned int write_flags; /* write file flags */
-	unsigned int read_flags; /* read file flags */
 
 	struct device *dt_device; /* device created from the device tree */
 	struct miscdevice miscdev;
@@ -165,7 +154,7 @@ static void reset_ip_core(struct axis_fifo *fifo)
 		  XLLF_INT_RPORE_MASK | XLLF_INT_RPUE_MASK |
 		  XLLF_INT_TPOE_MASK | XLLF_INT_TSE_MASK,
 		  fifo->base_addr + XLLF_IER_OFFSET);
-	iowrite32(XLLF_INT_ALL_MASK, fifo->base_addr + XLLF_ISR_OFFSET);
+	iowrite32(XLLF_INT_CLEAR_ALL, fifo->base_addr + XLLF_ISR_OFFSET);
 }
 
 /**
@@ -195,7 +184,7 @@ static ssize_t axis_fifo_read(struct file *f, char __user *buf,
 	int ret;
 	u32 tmp_buf[READ_BUF_SIZE];
 
-	if (fifo->read_flags & O_NONBLOCK) {
+	if (f->f_flags & O_NONBLOCK) {
 		/*
 		 * Device opened in non-blocking mode. Try to lock it and then
 		 * check if any packet is available.
@@ -337,7 +326,7 @@ static ssize_t axis_fifo_write(struct file *f, const char __user *buf,
 	if (words_to_write > (fifo->tx_fifo_depth - 4))
 		return -EINVAL;
 
-	if (fifo->write_flags & O_NONBLOCK) {
+	if (f->f_flags & O_NONBLOCK) {
 		/*
 		 * Device opened in non-blocking mode. Try to lock it and then
 		 * check if there is any room to write the given buffer.
@@ -396,106 +385,36 @@ end_unlock:
 
 static irqreturn_t axis_fifo_irq(int irq, void *dw)
 {
-	struct axis_fifo *fifo = (struct axis_fifo *)dw;
-	unsigned int pending_interrupts;
-
-	do {
-		pending_interrupts = ioread32(fifo->base_addr +
-					      XLLF_IER_OFFSET) &
-					      ioread32(fifo->base_addr
-					      + XLLF_ISR_OFFSET);
-		if (pending_interrupts & XLLF_INT_RC_MASK) {
-			/* packet received */
-
-			/* wake the reader process if it is waiting */
-			wake_up(&fifo->read_queue);
-
-			/* clear interrupt */
-			iowrite32(XLLF_INT_RC_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TC_MASK) {
-			/* packet sent */
-
-			/* wake the writer process if it is waiting */
-			wake_up(&fifo->write_queue);
-
-			iowrite32(XLLF_INT_TC_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TFPF_MASK) {
-			/* transmit fifo programmable full */
-
-			iowrite32(XLLF_INT_TFPF_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TFPE_MASK) {
-			/* transmit fifo programmable empty */
-
-			iowrite32(XLLF_INT_TFPE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RFPF_MASK) {
-			/* receive fifo programmable full */
-
-			iowrite32(XLLF_INT_RFPF_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RFPE_MASK) {
-			/* receive fifo programmable empty */
-
-			iowrite32(XLLF_INT_RFPE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TRC_MASK) {
-			/* transmit reset complete interrupt */
-
-			iowrite32(XLLF_INT_TRC_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RRC_MASK) {
-			/* receive reset complete interrupt */
-
-			iowrite32(XLLF_INT_RRC_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RPURE_MASK) {
-			/* receive fifo under-read error interrupt */
-			dev_err(fifo->dt_device,
-				"receive under-read interrupt\n");
-
-			iowrite32(XLLF_INT_RPURE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RPORE_MASK) {
-			/* receive over-read error interrupt */
-			dev_err(fifo->dt_device,
-				"receive over-read interrupt\n");
-
-			iowrite32(XLLF_INT_RPORE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_RPUE_MASK) {
-			/* receive underrun error interrupt */
-			dev_err(fifo->dt_device,
-				"receive underrun error interrupt\n");
-
-			iowrite32(XLLF_INT_RPUE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TPOE_MASK) {
-			/* transmit overrun error interrupt */
-			dev_err(fifo->dt_device,
-				"transmit overrun error interrupt\n");
-
-			iowrite32(XLLF_INT_TPOE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts & XLLF_INT_TSE_MASK) {
-			/* transmit length mismatch error interrupt */
-			dev_err(fifo->dt_device,
-				"transmit length mismatch error interrupt\n");
-
-			iowrite32(XLLF_INT_TSE_MASK & XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		} else if (pending_interrupts) {
-			/* unknown interrupt type */
-			dev_err(fifo->dt_device,
-				"unknown interrupt(s) 0x%x\n",
-				pending_interrupts);
-
-			iowrite32(XLLF_INT_ALL_MASK,
-				  fifo->base_addr + XLLF_ISR_OFFSET);
-		}
-	} while (pending_interrupts);
+	struct axis_fifo *fifo = dw;
+	u32 isr, ier, intr;
+
+	ier = ioread32(fifo->base_addr + XLLF_IER_OFFSET);
+	isr = ioread32(fifo->base_addr + XLLF_ISR_OFFSET);
+	intr = ier & isr;
+
+	if (intr & XLLF_INT_RC_MASK)
+		wake_up(&fifo->read_queue);
+
+	if (intr & XLLF_INT_TC_MASK)
+		wake_up(&fifo->write_queue);
+
+	if (intr & XLLF_INT_RPURE_MASK)
+		dev_err(fifo->dt_device, "receive under-read interrupt\n");
+
+	if (intr & XLLF_INT_RPORE_MASK)
+		dev_err(fifo->dt_device, "receive over-read interrupt\n");
+
+	if (intr & XLLF_INT_RPUE_MASK)
+		dev_err(fifo->dt_device, "receive underrun error interrupt\n");
+
+	if (intr & XLLF_INT_TPOE_MASK)
+		dev_err(fifo->dt_device, "transmit overrun error interrupt\n");
+
+	if (intr & XLLF_INT_TSE_MASK)
+		dev_err(fifo->dt_device,
+			"transmit length mismatch error interrupt\n");
+
+	iowrite32(XLLF_INT_CLEAR_ALL, fifo->base_addr + XLLF_ISR_OFFSET);
 
 	return IRQ_HANDLED;
 }
@@ -504,27 +423,15 @@ static int axis_fifo_open(struct inode *inod, struct file *f)
 {
 	struct axis_fifo *fifo = container_of(f->private_data,
 					      struct axis_fifo, miscdev);
+	unsigned int flags = f->f_flags & O_ACCMODE;
+
 	f->private_data = fifo;
 
-	if (((f->f_flags & O_ACCMODE) == O_WRONLY) ||
-	    ((f->f_flags & O_ACCMODE) == O_RDWR)) {
-		if (fifo->has_tx_fifo) {
-			fifo->write_flags = f->f_flags;
-		} else {
-			dev_err(fifo->dt_device, "tried to open device for write but the transmit fifo is disabled\n");
-			return -EPERM;
-		}
-	}
+	if ((flags == O_WRONLY || flags == O_RDWR) && !fifo->has_tx_fifo)
+		return -EPERM;
 
-	if (((f->f_flags & O_ACCMODE) == O_RDONLY) ||
-	    ((f->f_flags & O_ACCMODE) == O_RDWR)) {
-		if (fifo->has_rx_fifo) {
-			fifo->read_flags = f->f_flags;
-		} else {
-			dev_err(fifo->dt_device, "tried to open device for read but the receive fifo is disabled\n");
-			return -EPERM;
-		}
-	}
+	if ((flags == O_RDONLY || flags == O_RDWR) && !fifo->has_rx_fifo)
+		return -EPERM;
 
 	return 0;
 }
@@ -575,30 +482,14 @@ static void axis_fifo_debugfs_init(struct axis_fifo *fifo)
 			    &axis_fifo_debugfs_regs_fops);
 }
 
-/* read named property from the device tree */
-static int get_dts_property(struct axis_fifo *fifo,
-			    char *name, unsigned int *var)
-{
-	int rc;
-
-	rc = of_property_read_u32(fifo->dt_device->of_node, name, var);
-	if (rc) {
-		dev_err(fifo->dt_device, "couldn't read IP dts property '%s'",
-			name);
-		return rc;
-	}
-	dev_dbg(fifo->dt_device, "dts property '%s' = %u\n",
-		name, *var);
-
-	return 0;
-}
-
 static int axis_fifo_parse_dt(struct axis_fifo *fifo)
 {
 	int ret;
 	unsigned int value;
+	struct device_node *node = fifo->dt_device->of_node;
 
-	ret = get_dts_property(fifo, "xlnx,axi-str-rxd-tdata-width", &value);
+	ret = of_property_read_u32(node, "xlnx,axi-str-rxd-tdata-width",
+				   &value);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,axi-str-rxd-tdata-width property\n");
 		goto end;
@@ -608,7 +499,8 @@ static int axis_fifo_parse_dt(struct axis_fifo *fifo)
 		goto end;
 	}
 
-	ret = get_dts_property(fifo, "xlnx,axi-str-txd-tdata-width", &value);
+	ret = of_property_read_u32(node, "xlnx,axi-str-txd-tdata-width",
+				   &value);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,axi-str-txd-tdata-width property\n");
 		goto end;
@@ -618,30 +510,32 @@ static int axis_fifo_parse_dt(struct axis_fifo *fifo)
 		goto end;
 	}
 
-	ret = get_dts_property(fifo, "xlnx,rx-fifo-depth",
-			       &fifo->rx_fifo_depth);
+	ret = of_property_read_u32(node, "xlnx,rx-fifo-depth",
+				   &fifo->rx_fifo_depth);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,rx-fifo-depth property\n");
 		ret = -EIO;
 		goto end;
 	}
 
-	ret = get_dts_property(fifo, "xlnx,tx-fifo-depth",
-			       &fifo->tx_fifo_depth);
+	ret = of_property_read_u32(node, "xlnx,tx-fifo-depth",
+				   &fifo->tx_fifo_depth);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,tx-fifo-depth property\n");
 		ret = -EIO;
 		goto end;
 	}
 
-	ret = get_dts_property(fifo, "xlnx,use-rx-data", &fifo->has_rx_fifo);
+	ret = of_property_read_u32(node, "xlnx,use-rx-data",
+				   &fifo->has_rx_fifo);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,use-rx-data property\n");
 		ret = -EIO;
 		goto end;
 	}
 
-	ret = get_dts_property(fifo, "xlnx,use-tx-data", &fifo->has_tx_fifo);
+	ret = of_property_read_u32(node, "xlnx,use-tx-data",
+				   &fifo->has_tx_fifo);
 	if (ret) {
 		dev_err(fifo->dt_device, "missing xlnx,use-tx-data property\n");
 		ret = -EIO;
@@ -659,6 +553,7 @@ static int axis_fifo_probe(struct platform_device *pdev)
 	struct axis_fifo *fifo = NULL;
 	char *device_name;
 	int rc = 0; /* error return value */
+	int irq;
 
 	/* ----------------------------
 	 *     init wrapper device
@@ -693,8 +588,6 @@ static int axis_fifo_probe(struct platform_device *pdev)
 	if (IS_ERR(fifo->base_addr))
 		return PTR_ERR(fifo->base_addr);
 
-	dev_dbg(fifo->dt_device, "remapped memory to 0x%p\n", fifo->base_addr);
-
 	/* ----------------------------
 	 *          init IP
 	 * ----------------------------
@@ -712,17 +605,16 @@ static int axis_fifo_probe(struct platform_device *pdev)
 	 */
 
 	/* get IRQ resource */
-	rc = platform_get_irq(pdev, 0);
-	if (rc < 0)
-		return rc;
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0)
+		return irq;
 
 	/* request IRQ */
-	fifo->irq = rc;
-	rc = devm_request_irq(fifo->dt_device, fifo->irq, &axis_fifo_irq, 0,
+	rc = devm_request_irq(fifo->dt_device, irq, &axis_fifo_irq, 0,
 			      DRIVER_NAME, fifo);
 	if (rc) {
 		dev_err(fifo->dt_device, "couldn't allocate interrupt %i\n",
-			fifo->irq);
+			irq);
 		return rc;
 	}
 
@@ -764,6 +656,8 @@ static void axis_fifo_remove(struct platform_device *pdev)
 
 static const struct of_device_id axis_fifo_of_match[] = {
 	{ .compatible = "xlnx,axi-fifo-mm-s-4.1", },
+	{ .compatible = "xlnx,axi-fifo-mm-s-4.2", },
+	{ .compatible = "xlnx,axi-fifo-mm-s-4.3", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, axis_fifo_of_match);
@@ -806,4 +700,4 @@ module_exit(axis_fifo_exit);
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Jacob Feder <jacobsfeder@gmail.com>");
-MODULE_DESCRIPTION("Xilinx AXI-Stream FIFO v4.1 IP core driver");
+MODULE_DESCRIPTION("Xilinx AXI-Stream FIFO IP core driver");
diff --git a/drivers/staging/axis-fifo/axis-fifo.txt b/drivers/staging/axis-fifo/axis-fifo.txt
index 5828e1b8e822..413b81a53202 100644
--- a/drivers/staging/axis-fifo/axis-fifo.txt
+++ b/drivers/staging/axis-fifo/axis-fifo.txt
@@ -14,7 +14,10 @@ AXI4-Lite interface. DOES NOT support:
 	- AXI4 (non-lite)
 
 Required properties:
-- compatible: Should be "xlnx,axi-fifo-mm-s-4.1"
+- compatible: Should be one of:
+    "xlnx,axi-fifo-mm-s-4.1"
+    "xlnx,axi-fifo-mm-s-4.2"
+    "xlnx,axi-fifo-mm-s-4.3"
 - interrupt-names: Should be "interrupt"
 - interrupt-parent: Should be <&intc>
 - interrupts: Should contain interrupts lines.
diff --git a/drivers/staging/fbtft/fbtft-core.c b/drivers/staging/fbtft/fbtft-core.c
index 9e7b84071174..8a5ccc8ae0a1 100644
--- a/drivers/staging/fbtft/fbtft-core.c
+++ b/drivers/staging/fbtft/fbtft-core.c
@@ -1171,8 +1171,8 @@ int fbtft_probe_common(struct fbtft_display *display,
 	par->pdev = pdev;
 
 	if (display->buswidth == 0) {
-		dev_err(dev, "buswidth is not set\n");
-		return -EINVAL;
+		ret = dev_err_probe(dev, -EINVAL, "buswidth is not set\n");
+		goto out_release;
 	}
 
 	/* write register functions */
diff --git a/drivers/staging/greybus/uart.c b/drivers/staging/greybus/uart.c
index 10df5c37c83e..5cece0a6606f 100644
--- a/drivers/staging/greybus/uart.c
+++ b/drivers/staging/greybus/uart.c
@@ -879,14 +879,18 @@ static int gb_uart_probe(struct gbphy_device *gbphy_dev,
 	if (retval)
 		goto exit_put_port;
 
-	send_control(gb_tty, gb_tty->ctrlout);
+	retval = send_control(gb_tty, gb_tty->ctrlout);
+	if (retval)
+		goto exit_connection_disable;
 
 	/* initialize the uart to be 9600n81 */
 	gb_tty->line_coding.rate = cpu_to_le32(9600);
 	gb_tty->line_coding.format = GB_SERIAL_1_STOP_BITS;
 	gb_tty->line_coding.parity = GB_SERIAL_NO_PARITY;
 	gb_tty->line_coding.data_bits = 8;
-	send_line_coding(gb_tty);
+	retval = send_line_coding(gb_tty);
+	if (retval)
+		goto exit_connection_disable;
 
 	retval = gb_connection_enable(connection);
 	if (retval)
diff --git a/drivers/staging/most/Kconfig b/drivers/staging/most/Kconfig
index 6f420cbcdcff..e89658df6f12 100644
--- a/drivers/staging/most/Kconfig
+++ b/drivers/staging/most/Kconfig
@@ -24,6 +24,4 @@ source "drivers/staging/most/video/Kconfig"
 
 source "drivers/staging/most/dim2/Kconfig"
 
-source "drivers/staging/most/i2c/Kconfig"
-
 endif
diff --git a/drivers/staging/most/Makefile b/drivers/staging/most/Makefile
index 8b3fc5a7af51..e45084df7803 100644
--- a/drivers/staging/most/Makefile
+++ b/drivers/staging/most/Makefile
@@ -3,4 +3,3 @@
 obj-$(CONFIG_MOST_NET)	+= net/
 obj-$(CONFIG_MOST_VIDEO)	+= video/
 obj-$(CONFIG_MOST_DIM2)	+= dim2/
-obj-$(CONFIG_MOST_I2C)	+= i2c/
diff --git a/drivers/staging/most/i2c/Kconfig b/drivers/staging/most/i2c/Kconfig
deleted file mode 100644
index ff64283cbad1..000000000000
--- a/drivers/staging/most/i2c/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0
-#
-# MOST I2C configuration
-#
-
-config MOST_I2C
-	tristate "I2C"
-	depends on I2C
-	help
-	  Say Y here if you want to connect via I2C to network transceiver.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called most_i2c.
diff --git a/drivers/staging/most/i2c/Makefile b/drivers/staging/most/i2c/Makefile
deleted file mode 100644
index 71099dd0f85b..000000000000
--- a/drivers/staging/most/i2c/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0
-obj-$(CONFIG_MOST_I2C) += most_i2c.o
-
-most_i2c-objs := i2c.o
diff --git a/drivers/staging/most/i2c/i2c.c b/drivers/staging/most/i2c/i2c.c
deleted file mode 100644
index 184b2dd11fc3..000000000000
--- a/drivers/staging/most/i2c/i2c.c
+++ /dev/null
@@ -1,374 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * i2c.c - Hardware Dependent Module for I2C Interface
- *
- * Copyright (C) 2013-2015, Microchip Technology Germany II GmbH & Co. KG
- */
-
-#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
-
-#include <linux/init.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/i2c.h>
-#include <linux/interrupt.h>
-#include <linux/err.h>
-#include <linux/most.h>
-
-enum { CH_RX, CH_TX, NUM_CHANNELS };
-
-#define MAX_BUFFERS_CONTROL 32
-#define MAX_BUF_SIZE_CONTROL 256
-
-/**
- * list_first_mbo - get the first mbo from a list
- * @ptr:	the list head to take the mbo from.
- */
-#define list_first_mbo(ptr) \
-	list_first_entry(ptr, struct mbo, list)
-
-static unsigned int polling_rate;
-module_param(polling_rate, uint, 0644);
-MODULE_PARM_DESC(polling_rate, "Polling rate [Hz]. Default = 0 (use IRQ)");
-
-struct hdm_i2c {
-	struct most_interface most_iface;
-	struct most_channel_capability capabilities[NUM_CHANNELS];
-	struct i2c_client *client;
-	struct rx {
-		struct delayed_work dwork;
-		struct list_head list;
-		bool int_disabled;
-		unsigned int delay;
-	} rx;
-	char name[64];
-};
-
-static inline struct hdm_i2c *to_hdm(struct most_interface *iface)
-{
-	return container_of(iface, struct hdm_i2c, most_iface);
-}
-
-static irqreturn_t most_irq_handler(int, void *);
-static void pending_rx_work(struct work_struct *);
-
-/**
- * configure_channel - called from MOST core to configure a channel
- * @most_iface: interface the channel belongs to
- * @ch_idx: channel to be configured
- * @channel_config: structure that holds the configuration information
- *
- * Return 0 on success, negative on failure.
- *
- * Receives configuration information from MOST core and initialize the
- * corresponding channel.
- */
-static int configure_channel(struct most_interface *most_iface,
-			     int ch_idx,
-			     struct most_channel_config *channel_config)
-{
-	int ret;
-	struct hdm_i2c *dev = to_hdm(most_iface);
-	unsigned int delay, pr;
-
-	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
-
-	if (channel_config->data_type != MOST_CH_CONTROL) {
-		pr_err("bad data type for channel %d\n", ch_idx);
-		return -EPERM;
-	}
-
-	if (channel_config->direction != dev->capabilities[ch_idx].direction) {
-		pr_err("bad direction for channel %d\n", ch_idx);
-		return -EPERM;
-	}
-
-	if (channel_config->direction == MOST_CH_RX) {
-		if (!polling_rate) {
-			if (dev->client->irq <= 0) {
-				pr_err("bad irq: %d\n", dev->client->irq);
-				return -ENOENT;
-			}
-			dev->rx.int_disabled = false;
-			ret = request_irq(dev->client->irq, most_irq_handler, 0,
-					  dev->client->name, dev);
-			if (ret) {
-				pr_err("request_irq(%d) failed: %d\n",
-				       dev->client->irq, ret);
-				return ret;
-			}
-		} else {
-			delay = msecs_to_jiffies(MSEC_PER_SEC / polling_rate);
-			dev->rx.delay = delay ? delay : 1;
-			pr = MSEC_PER_SEC / jiffies_to_msecs(dev->rx.delay);
-			pr_info("polling rate is %u Hz\n", pr);
-		}
-	}
-
-	return 0;
-}
-
-/**
- * enqueue - called from MOST core to enqueue a buffer for data transfer
- * @most_iface: intended interface
- * @ch_idx: ID of the channel the buffer is intended for
- * @mbo: pointer to the buffer object
- *
- * Return 0 on success, negative on failure.
- *
- * Transmit the data over I2C if it is a "write" request or push the buffer into
- * list if it is an "read" request
- */
-static int enqueue(struct most_interface *most_iface,
-		   int ch_idx, struct mbo *mbo)
-{
-	struct hdm_i2c *dev = to_hdm(most_iface);
-	int ret;
-
-	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
-
-	if (ch_idx == CH_RX) {
-		/* RX */
-		if (!polling_rate)
-			disable_irq(dev->client->irq);
-		cancel_delayed_work_sync(&dev->rx.dwork);
-		list_add_tail(&mbo->list, &dev->rx.list);
-		if (dev->rx.int_disabled || polling_rate)
-			pending_rx_work(&dev->rx.dwork.work);
-		if (!polling_rate)
-			enable_irq(dev->client->irq);
-	} else {
-		/* TX */
-		ret = i2c_master_send(dev->client, mbo->virt_address,
-				      mbo->buffer_length);
-		if (ret <= 0) {
-			mbo->processed_length = 0;
-			mbo->status = MBO_E_INVAL;
-		} else {
-			mbo->processed_length = mbo->buffer_length;
-			mbo->status = MBO_SUCCESS;
-		}
-		mbo->complete(mbo);
-	}
-
-	return 0;
-}
-
-/**
- * poison_channel - called from MOST core to poison buffers of a channel
- * @most_iface: pointer to the interface the channel to be poisoned belongs to
- * @ch_idx: corresponding channel ID
- *
- * Return 0 on success, negative on failure.
- *
- * If channel direction is RX, complete the buffers in list with
- * status MBO_E_CLOSE
- */
-static int poison_channel(struct most_interface *most_iface,
-			  int ch_idx)
-{
-	struct hdm_i2c *dev = to_hdm(most_iface);
-	struct mbo *mbo;
-
-	BUG_ON(ch_idx < 0 || ch_idx >= NUM_CHANNELS);
-
-	if (ch_idx == CH_RX) {
-		if (!polling_rate)
-			free_irq(dev->client->irq, dev);
-		cancel_delayed_work_sync(&dev->rx.dwork);
-
-		while (!list_empty(&dev->rx.list)) {
-			mbo = list_first_mbo(&dev->rx.list);
-			list_del(&mbo->list);
-
-			mbo->processed_length = 0;
-			mbo->status = MBO_E_CLOSE;
-			mbo->complete(mbo);
-		}
-	}
-
-	return 0;
-}
-
-static void do_rx_work(struct hdm_i2c *dev)
-{
-	struct mbo *mbo;
-	unsigned char msg[MAX_BUF_SIZE_CONTROL];
-	int ret;
-	u16 pml, data_size;
-
-	/* Read PML (2 bytes) */
-	ret = i2c_master_recv(dev->client, msg, 2);
-	if (ret <= 0) {
-		pr_err("Failed to receive PML\n");
-		return;
-	}
-
-	pml = (msg[0] << 8) | msg[1];
-	if (!pml)
-		return;
-
-	data_size = pml + 2;
-
-	/* Read the whole message, including PML */
-	ret = i2c_master_recv(dev->client, msg, data_size);
-	if (ret <= 0) {
-		pr_err("Failed to receive a Port Message\n");
-		return;
-	}
-
-	mbo = list_first_mbo(&dev->rx.list);
-	list_del(&mbo->list);
-
-	mbo->processed_length = min(data_size, mbo->buffer_length);
-	memcpy(mbo->virt_address, msg, mbo->processed_length);
-	mbo->status = MBO_SUCCESS;
-	mbo->complete(mbo);
-}
-
-/**
- * pending_rx_work - Read pending messages through I2C
- * @work: definition of this work item
- *
- * Invoked by the Interrupt Service Routine, most_irq_handler()
- */
-static void pending_rx_work(struct work_struct *work)
-{
-	struct hdm_i2c *dev = container_of(work, struct hdm_i2c, rx.dwork.work);
-
-	if (list_empty(&dev->rx.list))
-		return;
-
-	do_rx_work(dev);
-
-	if (polling_rate) {
-		schedule_delayed_work(&dev->rx.dwork, dev->rx.delay);
-	} else {
-		dev->rx.int_disabled = false;
-		enable_irq(dev->client->irq);
-	}
-}
-
-/*
- * most_irq_handler - Interrupt Service Routine
- * @irq: irq number
- * @_dev: private data
- *
- * Schedules a delayed work
- *
- * By default the interrupt line behavior is Active Low. Once an interrupt is
- * generated by the device, until driver clears the interrupt (by reading
- * the PMP message), device keeps the interrupt line in low state. Since i2c
- * read is done in work queue, the interrupt line must be disabled temporarily
- * to avoid ISR being called repeatedly. Re-enable the interrupt in workqueue,
- * after reading the message.
- *
- * Note: If we use the interrupt line in Falling edge mode, there is a
- * possibility to miss interrupts when ISR is getting executed.
- *
- */
-static irqreturn_t most_irq_handler(int irq, void *_dev)
-{
-	struct hdm_i2c *dev = _dev;
-
-	disable_irq_nosync(irq);
-	dev->rx.int_disabled = true;
-	schedule_delayed_work(&dev->rx.dwork, 0);
-
-	return IRQ_HANDLED;
-}
-
-/*
- * i2c_probe - i2c probe handler
- * @client: i2c client device structure
- * @id: i2c client device id
- *
- * Return 0 on success, negative on failure.
- *
- * Register the i2c client device as a MOST interface
- */
-static int i2c_probe(struct i2c_client *client)
-{
-	struct hdm_i2c *dev;
-	int ret, i;
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
-		return -ENOMEM;
-
-	/* ID format: i2c-<bus>-<address> */
-	snprintf(dev->name, sizeof(dev->name), "i2c-%d-%04x",
-		 client->adapter->nr, client->addr);
-
-	for (i = 0; i < NUM_CHANNELS; i++) {
-		dev->capabilities[i].data_type = MOST_CH_CONTROL;
-		dev->capabilities[i].num_buffers_packet = MAX_BUFFERS_CONTROL;
-		dev->capabilities[i].buffer_size_packet = MAX_BUF_SIZE_CONTROL;
-	}
-	dev->capabilities[CH_RX].direction = MOST_CH_RX;
-	dev->capabilities[CH_RX].name_suffix = "rx";
-	dev->capabilities[CH_TX].direction = MOST_CH_TX;
-	dev->capabilities[CH_TX].name_suffix = "tx";
-
-	dev->most_iface.interface = ITYPE_I2C;
-	dev->most_iface.description = dev->name;
-	dev->most_iface.num_channels = NUM_CHANNELS;
-	dev->most_iface.channel_vector = dev->capabilities;
-	dev->most_iface.configure = configure_channel;
-	dev->most_iface.enqueue = enqueue;
-	dev->most_iface.poison_channel = poison_channel;
-
-	INIT_LIST_HEAD(&dev->rx.list);
-
-	INIT_DELAYED_WORK(&dev->rx.dwork, pending_rx_work);
-
-	dev->client = client;
-	i2c_set_clientdata(client, dev);
-
-	ret = most_register_interface(&dev->most_iface);
-	if (ret) {
-		pr_err("Failed to register i2c as a MOST interface\n");
-		kfree(dev);
-		return ret;
-	}
-
-	return 0;
-}
-
-/*
- * i2c_remove - i2c remove handler
- * @client: i2c client device structure
- *
- * Return 0 on success.
- *
- * Unregister the i2c client device as a MOST interface
- */
-static void i2c_remove(struct i2c_client *client)
-{
-	struct hdm_i2c *dev = i2c_get_clientdata(client);
-
-	most_deregister_interface(&dev->most_iface);
-	kfree(dev);
-}
-
-static const struct i2c_device_id i2c_id[] = {
-	{ "most_i2c" },
-	{ } /* Terminating entry */
-};
-
-MODULE_DEVICE_TABLE(i2c, i2c_id);
-
-static struct i2c_driver i2c_driver = {
-	.driver = {
-		.name = "hdm_i2c",
-	},
-	.probe = i2c_probe,
-	.remove = i2c_remove,
-	.id_table = i2c_id,
-};
-
-module_i2c_driver(i2c_driver);
-
-MODULE_AUTHOR("Andrey Shvetsov <andrey.shvetsov@k2l.de>");
-MODULE_DESCRIPTION("I2C Hardware Dependent Module");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/nvec/nvec_ps2.c b/drivers/staging/nvec/nvec_ps2.c
index 575233fa1677..2db57795ea2f 100644
--- a/drivers/staging/nvec/nvec_ps2.c
+++ b/drivers/staging/nvec/nvec_ps2.c
@@ -23,14 +23,6 @@
 #define DISABLE_MOUSE	0xf5
 #define PSMOUSE_RST	0xff
 
-#ifdef NVEC_PS2_DEBUG
-#define NVEC_PHD(str, buf, len) \
-	print_hex_dump(KERN_DEBUG, str, DUMP_PREFIX_NONE, \
-			16, 1, buf, len, false)
-#else
-#define NVEC_PHD(str, buf, len) do { } while (0)
-#endif
-
 enum ps2_subcmds {
 	SEND_COMMAND = 1,
 	RECEIVE_N,
@@ -70,18 +62,14 @@ static int nvec_ps2_notifier(struct notifier_block *nb,
 	case NVEC_PS2_EVT:
 		for (i = 0; i < msg[1]; i++)
 			serio_interrupt(ps2_dev.ser_dev, msg[2 + i], 0);
-		NVEC_PHD("ps/2 mouse event: ", &msg[2], msg[1]);
 		return NOTIFY_STOP;
 
 	case NVEC_PS2:
 		if (msg[2] == 1) {
 			for (i = 0; i < (msg[1] - 2); i++)
 				serio_interrupt(ps2_dev.ser_dev, msg[i + 4], 0);
-			NVEC_PHD("ps/2 mouse reply: ", &msg[4], msg[1] - 2);
 		}
 
-		else if (msg[1] != 2) /* !ack */
-			NVEC_PHD("unhandled mouse event: ", msg, msg[1] + 2);
 		return NOTIFY_STOP;
 	}
 
diff --git a/drivers/staging/rtl8723bs/core/rtw_ap.c b/drivers/staging/rtl8723bs/core/rtw_ap.c
index 0908f2234f67..67197c7d4a4d 100644
--- a/drivers/staging/rtl8723bs/core/rtw_ap.c
+++ b/drivers/staging/rtl8723bs/core/rtw_ap.c
@@ -391,8 +391,6 @@ void update_bmc_sta(struct adapter *padapter)
 
 		memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
 
-		/* psta->dot118021XPrivacy = _NO_PRIVACY_;//!!! remove it, because it has been set before this. */
-
 		/* prepare for add_RATid */
 		supportRateNum = rtw_get_rateset_len((u8 *)&pcur_network->supported_rates);
 		network_type = rtw_check_network_type((u8 *)&pcur_network->supported_rates,
@@ -436,7 +434,6 @@ void update_bmc_sta(struct adapter *padapter)
 		spin_lock_bh(&psta->lock);
 		psta->state = _FW_LINKED;
 		spin_unlock_bh(&psta->lock);
-
 	}
 }
 
@@ -480,14 +477,14 @@ void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
 		/* check if sta supports rx ampdu */
 		phtpriv_sta->ampdu_enable = phtpriv_ap->ampdu_enable;
 
-		phtpriv_sta->rx_ampdu_min_spacing = (
-			phtpriv_sta->ht_cap.ampdu_params_info & IEEE80211_HT_CAP_AMPDU_DENSITY
-		) >> 2;
+		phtpriv_sta->rx_ampdu_min_spacing =
+			(phtpriv_sta->ht_cap.ampdu_params_info &
+			 IEEE80211_HT_CAP_AMPDU_DENSITY) >> 2;
 
 		/*  bwmode */
-		if ((
-			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
-		) & cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))
+		if ((phtpriv_sta->ht_cap.cap_info &
+		     phtpriv_ap->ht_cap.cap_info) &
+		    cpu_to_le16(IEEE80211_HT_CAP_SUP_WIDTH))
 			psta->bw_mode = CHANNEL_WIDTH_40;
 		else
 			psta->bw_mode = CHANNEL_WIDTH_20;
@@ -498,15 +495,15 @@ void update_sta_info_apmode(struct adapter *padapter, struct sta_info *psta)
 		phtpriv_sta->ch_offset = pmlmeext->cur_ch_offset;
 
 		/* check if sta support s Short GI 20M */
-		if ((
-			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
-		) & cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
+		if ((phtpriv_sta->ht_cap.cap_info &
+		     phtpriv_ap->ht_cap.cap_info) &
+		    cpu_to_le16(IEEE80211_HT_CAP_SGI_20))
 			phtpriv_sta->sgi_20m = true;
 
 		/* check if sta support s Short GI 40M */
-		if ((
-			phtpriv_sta->ht_cap.cap_info & phtpriv_ap->ht_cap.cap_info
-		) & cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) {
+		if ((phtpriv_sta->ht_cap.cap_info &
+		     phtpriv_ap->ht_cap.cap_info) &
+		    cpu_to_le16(IEEE80211_HT_CAP_SGI_40)) {
 			if (psta->bw_mode == CHANNEL_WIDTH_40) /* according to psta->bw_mode */
 				phtpriv_sta->sgi_40m = true;
 			else
@@ -625,9 +622,9 @@ static void update_hw_ht_param(struct adapter *padapter)
 	/*  */
 	/*  Config SM Power Save setting */
 	/*  */
-	pmlmeinfo->SM_PS = (le16_to_cpu(
-		pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info
-	) & 0x0C) >> 2;
+	pmlmeinfo->SM_PS =
+		(le16_to_cpu(pmlmeinfo->HT_caps.u.HT_cap_element.HT_caps_info) &
+		 0x0C) >> 2;
 
 	/*  */
 	/*  Config current HT Protection mode. */
@@ -658,9 +655,12 @@ void start_bss_network(struct adapter *padapter)
 	cur_bwmode = CHANNEL_WIDTH_20;
 	cur_ch_offset = HAL_PRIME_CHNL_OFFSET_DONT_CARE;
 
-	/* check if there is wps ie, */
-	/* if there is wpsie in beacon, the hostapd will update beacon twice when stating hostapd, */
-	/* and at first time the security ie (RSN/WPA IE) will not include in beacon. */
+	/*
+	 * check if there is wps ie,
+	 * if there is wpsie in beacon,
+	 * the hostapd will update beacon twice when stating hostapd,
+	 * and at first time the security ie (RSN/WPA IE) will not include in beacon.
+	 */
 	if (!rtw_get_wps_ie(pnetwork->ies + _FIXED_IE_LENGTH_,
 			    pnetwork->ie_length - _FIXED_IE_LENGTH_, NULL, NULL))
 		pmlmeext->bstart_bss = true;
@@ -705,9 +705,8 @@ void start_bss_network(struct adapter *padapter)
 	rtw_hal_set_hwreg(padapter, HW_VAR_AC_PARAM_BK, (u8 *)(&acparm));
 
 	/* Set Security */
-	val8 = (
-		psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X
-	) ? 0xcc : 0xcf;
+	val8 = (psecuritypriv->dot11AuthAlgrthm ==
+		dot11AuthAlgrthm_8021X) ? 0xcc : 0xcf;
 	rtw_hal_set_hwreg(padapter, HW_VAR_SEC_CFG, (u8 *)(&val8));
 
 	/* Beacon Control related register */
@@ -778,14 +777,12 @@ void start_bss_network(struct adapter *padapter)
 	update_wireless_mode(padapter);
 
 	/* update RRSR after set channel and bandwidth */
-	UpdateBrateTbl(padapter, pnetwork->supported_rates);
+	update_basic_rate_table(padapter, pnetwork->supported_rates);
 	rtw_hal_set_hwreg(padapter, HW_VAR_BASIC_RATE, pnetwork->supported_rates);
 
 	/* update capability after cur_wireless_mode updated */
-	update_capinfo(
-		padapter,
-		rtw_get_capability((struct wlan_bssid_ex *)pnetwork)
-	);
+	update_capinfo(padapter,
+		       rtw_get_capability((struct wlan_bssid_ex *)pnetwork));
 
 	if (pmlmeext->bstart_bss) {
 		update_beacon(padapter, WLAN_EID_TIM, NULL, true);
@@ -841,7 +838,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	memcpy(pbss_network->mac_address, myid(&padapter->eeprompriv), ETH_ALEN);
 
 	/* beacon interval */
-	p = rtw_get_beacon_interval_from_ie(ie);/* ie + 8;	8: TimeStamp, 2: Beacon Interval 2:Capability */
+	/* ie + 8;	8: TimeStamp, 2: Beacon Interval 2:Capability */
+	p = rtw_get_beacon_interval_from_ie(ie);
 	/* pbss_network->configuration.beacon_period = le16_to_cpu(*(unsigned short*)p); */
 	pbss_network->configuration.beacon_period = get_unaligned_le16(p);
 
@@ -851,12 +849,10 @@ 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_,
-		WLAN_EID_SSID,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_SSID,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		memset(&pbss_network->ssid, 0, sizeof(struct ndis_802_11_ssid));
 		memcpy(pbss_network->ssid.ssid, (p + 2), ie_len);
@@ -866,11 +862,9 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	/* channel */
 	channel = 0;
 	pbss_network->configuration.length = 0;
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_DS_PARAMS, &ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_DS_PARAMS, &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		channel = *(p + 2);
 
@@ -878,24 +872,20 @@ 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_,
-		WLAN_EID_SUPP_RATES,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_SUPP_RATES,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p) {
 		memcpy(supportRate, p + 2, ie_len);
 		supportRateNum = ie_len;
 	}
 
 	/* get ext_supported rates */
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_EXT_SUPP_RATES,
-		&ie_len,
-		pbss_network->ie_length - _BEACON_IE_OFFSET_
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_EXT_SUPP_RATES,
+		       &ie_len,
+		       pbss_network->ie_length - _BEACON_IE_OFFSET_);
 	if (p) {
 		memcpy(supportRate + supportRateNum, p + 2, ie_len);
 		supportRateNum += ie_len;
@@ -906,12 +896,10 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	rtw_set_supported_rate(pbss_network->supported_rates, network_type);
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_ERP_INFO,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_ERP_INFO,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		ERP_IE_handler(padapter, (struct ndis_80211_var_ie *)p);
 
@@ -927,20 +915,16 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	group_cipher = 0; pairwise_cipher = 0;
 	psecuritypriv->wpa2_group_cipher = _NO_PRIVACY_;
 	psecuritypriv->wpa2_pairwise_cipher = _NO_PRIVACY_;
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_RSN,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_RSN,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
-		if (rtw_parse_wpa2_ie(
-			p,
-			ie_len + 2,
-			&group_cipher,
-			&pairwise_cipher,
-			NULL
-		) == _SUCCESS) {
+		if (rtw_parse_wpa2_ie(p,
+				      ie_len + 2,
+				      &group_cipher,
+				      &pairwise_cipher,
+				      NULL) == _SUCCESS) {
 			psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
 
 			psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
@@ -957,20 +941,16 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	psecuritypriv->wpa_group_cipher = _NO_PRIVACY_;
 	psecuritypriv->wpa_pairwise_cipher = _NO_PRIVACY_;
 	for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
-		p = rtw_get_ie(
-			p,
-			WLAN_EID_VENDOR_SPECIFIC,
-			&ie_len,
-			(pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2))
-		);
+		p = rtw_get_ie(p,
+			       WLAN_EID_VENDOR_SPECIFIC,
+			       &ie_len,
+			       (pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2)));
 		if ((p) && (!memcmp(p + 2, OUI1, 4))) {
-			if (rtw_parse_wpa_ie(
-				p,
-				ie_len + 2,
-				&group_cipher,
-				&pairwise_cipher,
-				NULL
-			) == _SUCCESS) {
+			if (rtw_parse_wpa_ie(p,
+					     ie_len + 2,
+					     &group_cipher,
+					     &pairwise_cipher,
+					     NULL) == _SUCCESS) {
 				psecuritypriv->dot11AuthAlgrthm = dot11AuthAlgrthm_8021X;
 
 				psecuritypriv->dot8021xalg = 1;/* psk,  todo:802.1x */
@@ -993,12 +973,11 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	pmlmepriv->qospriv.qos_option = 0;
 	if (pregistrypriv->wmm_enable) {
 		for (p = ie + _BEACON_IE_OFFSET_; ; p += (ie_len + 2)) {
-			p = rtw_get_ie(
-				p,
-				WLAN_EID_VENDOR_SPECIFIC,
-				&ie_len,
-				(pbss_network->ie_length - _BEACON_IE_OFFSET_ - (ie_len + 2))
-			);
+			p = rtw_get_ie(p,
+				       WLAN_EID_VENDOR_SPECIFIC,
+				       &ie_len,
+				       (pbss_network->ie_length -
+					_BEACON_IE_OFFSET_ - (ie_len + 2)));
 			if ((p) && !memcmp(p + 2, WMM_PARA_IE, 6)) {
 				pmlmepriv->qospriv.qos_option = 1;
 
@@ -1020,12 +999,10 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* parsing HT_CAP_IE */
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_HT_CAPABILITY,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_HT_CAPABILITY,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0) {
 		u8 max_rx_ampdu_factor = 0;
 		struct ieee80211_ht_cap *pht_cap = (struct ieee80211_ht_cap *)(p + 2);
@@ -1052,9 +1029,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 		if (!TEST_FLAG(pmlmepriv->htpriv.stbc_cap, STBC_HT_ENABLE_RX))
 			pht_cap->cap_info &= cpu_to_le16(~(IEEE80211_HT_CAP_RX_STBC_3R));
 
-		pht_cap->ampdu_params_info &= ~(
-			IEEE80211_HT_CAP_AMPDU_FACTOR | IEEE80211_HT_CAP_AMPDU_DENSITY
-		);
+		pht_cap->ampdu_params_info &= ~(IEEE80211_HT_CAP_AMPDU_FACTOR |
+						IEEE80211_HT_CAP_AMPDU_DENSITY);
 
 		if ((psecuritypriv->wpa_pairwise_cipher & WPA_CIPHER_CCMP) ||
 		    (psecuritypriv->wpa2_pairwise_cipher & WPA_CIPHER_CCMP)) {
@@ -1065,14 +1041,12 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 						       0x00);
 		}
 
-		rtw_hal_get_def_var(
-			padapter,
-			HW_VAR_MAX_RX_AMPDU_FACTOR,
-			&max_rx_ampdu_factor
-		);
-		pht_cap->ampdu_params_info |= (
-			IEEE80211_HT_CAP_AMPDU_FACTOR & max_rx_ampdu_factor
-		); /* set  Max Rx AMPDU size  to 64K */
+		rtw_hal_get_def_var(padapter,
+				    HW_VAR_MAX_RX_AMPDU_FACTOR,
+				    &max_rx_ampdu_factor);
+		/* set Max Rx AMPDU size to 64K */
+		pht_cap->ampdu_params_info |= (IEEE80211_HT_CAP_AMPDU_FACTOR &
+					       max_rx_ampdu_factor);
 
 		pht_cap->mcs.rx_mask[0] = 0xff;
 		pht_cap->mcs.rx_mask[1] = 0x0;
@@ -1081,12 +1055,10 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	}
 
 	/* parsing HT_INFO_IE */
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_HT_OPERATION,
-		&ie_len,
-		(pbss_network->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_HT_OPERATION,
+		       &ie_len,
+		       (pbss_network->ie_length - _BEACON_IE_OFFSET_));
 	if (p && ie_len > 0)
 		pHT_info_ie = p;
 
@@ -1128,9 +1100,8 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 		HT_info_handler(padapter, (struct ndis_80211_var_ie *)pHT_info_ie);
 	}
 
-	pbss_network->length = get_wlan_bssid_ex_sz(
-		(struct wlan_bssid_ex  *)pbss_network
-	);
+	pbss_network->length =
+		get_wlan_bssid_ex_sz((struct wlan_bssid_ex *)pbss_network);
 
 	/* issue beacon to start bss network */
 	/* start_bss_network(padapter, (u8 *)pbss_network); */
@@ -1147,7 +1118,7 @@ int rtw_check_beacon_data(struct adapter *padapter, u8 *pbuf,  int len)
 	/*  update AP's sta info */
 	update_ap_info(padapter, psta);
 
-	psta->state |= WIFI_AP_STATE;		/* Aries, add, fix bug of flush_cam_entry at STOP AP mode , 0724 */
+	psta->state |= WIFI_AP_STATE;
 	rtw_indicate_connect(padapter);
 
 	pmlmepriv->cur_network.join_res = true;/* for check if already set beacon */
@@ -1237,10 +1208,8 @@ void rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
 	list_for_each_safe(plist, tmp, phead) {
 		paclnode = list_entry(plist, struct rtw_wlan_acl_node, list);
 
-		if (
-			!memcmp(paclnode->addr, addr, ETH_ALEN) ||
-			is_broadcast_ether_addr(addr)
-		) {
+		if (!memcmp(paclnode->addr, addr, ETH_ALEN) ||
+		    is_broadcast_ether_addr(addr)) {
 			if (paclnode->valid) {
 				paclnode->valid = false;
 
@@ -1252,7 +1221,6 @@ void rtw_acl_remove_sta(struct adapter *padapter, u8 *addr)
 	}
 
 	spin_unlock_bh(&pacl_node_q->lock);
-
 }
 
 u8 rtw_ap_set_pairwise_key(struct adapter *padapter, struct sta_info *psta)
@@ -1290,13 +1258,11 @@ exit:
 	return res;
 }
 
-static int rtw_ap_set_key(
-	struct adapter *padapter,
-	u8 *key,
-	u8 alg,
-	int keyid,
-	u8 set_tx
-)
+static int rtw_ap_set_key(struct adapter *padapter,
+			  u8 *key,
+			  u8 alg,
+			  int keyid,
+			  u8 set_tx)
 {
 	u8 keylen;
 	struct cmd_obj *pcmd;
@@ -1360,13 +1326,11 @@ int rtw_ap_set_group_key(struct adapter *padapter, u8 *key, u8 alg, int keyid)
 	return rtw_ap_set_key(padapter, key, alg, keyid, 1);
 }
 
-int rtw_ap_set_wep_key(
-	struct adapter *padapter,
-	u8 *key,
-	u8 keylen,
-	int keyid,
-	u8 set_tx
-)
+int rtw_ap_set_wep_key(struct adapter *padapter,
+		       u8 *key,
+		       u8 keylen,
+		       int keyid,
+		       u8 set_tx)
 {
 	u8 alg;
 
@@ -1401,21 +1365,18 @@ static void update_bcn_erpinfo_ie(struct adapter *padapter)
 		return;
 
 	/* parsing ERP_IE */
-	p = rtw_get_ie(
-		ie + _BEACON_IE_OFFSET_,
-		WLAN_EID_ERP_INFO,
-		&len,
-		(pnetwork->ie_length - _BEACON_IE_OFFSET_)
-	);
+	p = rtw_get_ie(ie + _BEACON_IE_OFFSET_,
+		       WLAN_EID_ERP_INFO,
+		       &len,
+		       (pnetwork->ie_length - _BEACON_IE_OFFSET_));
 	if (p && len > 0) {
 		struct ndis_80211_var_ie *pIE = (struct ndis_80211_var_ie *)p;
 
 		if (pmlmepriv->num_sta_non_erp == 1)
 			pIE->data[0] |= RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION;
 		else
-			pIE->data[0] &= ~(
-				RTW_ERP_INFO_NON_ERP_PRESENT | RTW_ERP_INFO_USE_PROTECTION
-			);
+			pIE->data[0] &= ~(RTW_ERP_INFO_NON_ERP_PRESENT |
+					  RTW_ERP_INFO_USE_PROTECTION);
 
 		if (pmlmepriv->num_sta_no_short_preamble > 0)
 			pIE->data[0] |= RTW_ERP_INFO_BARKER_PREAMBLE_MODE;
@@ -1461,12 +1422,10 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 	unsigned char *ie = pnetwork->ies;
 	u32 ielen = pnetwork->ie_length;
 
-	pwps_ie = rtw_get_wps_ie(
-		ie + _FIXED_IE_LENGTH_,
-		ielen - _FIXED_IE_LENGTH_,
-		NULL,
-		&wps_ielen
-	);
+	pwps_ie = rtw_get_wps_ie(ie + _FIXED_IE_LENGTH_,
+				 ielen - _FIXED_IE_LENGTH_,
+				 NULL,
+				 &wps_ielen);
 
 	if (!pwps_ie || wps_ielen == 0)
 		return;
@@ -1490,7 +1449,7 @@ static void update_bcn_wps_ie(struct adapter *padapter)
 	wps_ielen = (uint)pwps_ie_src[1];/* to get ie data len */
 	if ((wps_offset + wps_ielen + 2 + remainder_ielen) <= MAX_IE_SZ) {
 		memcpy(pwps_ie, pwps_ie_src, wps_ielen + 2);
-		pwps_ie += (wps_ielen+2);
+		pwps_ie += (wps_ielen + 2);
 
 		if (pbackup_remainder_ie)
 			memcpy(pwps_ie, pbackup_remainder_ie, remainder_ielen);
@@ -1651,9 +1610,9 @@ static int rtw_ht_operation_update(struct adapter *padapter)
 	if (pmlmepriv->num_sta_no_ht ||
 	    (pmlmepriv->ht_op_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT))
 		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONHT_MIXED;
-	else if (
-		(le16_to_cpu(phtpriv_ap->ht_cap.cap_info) & IEEE80211_HT_CAP_SUP_WIDTH)
-		&& pmlmepriv->num_sta_ht_20mhz)
+	else if ((le16_to_cpu(phtpriv_ap->ht_cap.cap_info) &
+		  IEEE80211_HT_CAP_SUP_WIDTH) &&
+		 pmlmepriv->num_sta_ht_20mhz)
 		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_20MHZ;
 	else if (pmlmepriv->olbc_ht)
 		new_op_mode = IEEE80211_HT_OP_MODE_PROTECTION_NONMEMBER;
@@ -1874,12 +1833,10 @@ u8 bss_cap_update_on_sta_leave(struct adapter *padapter, struct sta_info *psta)
 	return beacon_updated;
 }
 
-u8 ap_free_sta(
-	struct adapter *padapter,
-	struct sta_info *psta,
-	bool active,
-	u16 reason
-)
+u8 ap_free_sta(struct adapter *padapter,
+	       struct sta_info *psta,
+	       bool active,
+	       u16 reason)
 {
 	u8 beacon_updated = false;
 
@@ -1993,6 +1950,7 @@ void ap_sta_info_defer_update(struct adapter *padapter, struct sta_info *psta)
 		add_RATid(padapter, psta, 0);/* DM_RATR_STA_INIT */
 	}
 }
+
 /* restore hw setting from sw data structures */
 void rtw_ap_restore_network(struct adapter *padapter)
 {
@@ -2007,25 +1965,21 @@ void rtw_ap_restore_network(struct adapter *padapter)
 
 	rtw_setopmode_cmd(padapter, Ndis802_11APMode, false);
 
-	set_channel_bwmode(
-		padapter,
-		pmlmeext->cur_channel,
-		pmlmeext->cur_ch_offset,
-		pmlmeext->cur_bwmode
-	);
+	set_channel_bwmode(padapter,
+			   pmlmeext->cur_channel,
+			   pmlmeext->cur_ch_offset,
+			   pmlmeext->cur_bwmode);
 
 	start_bss_network(padapter);
 
 	if ((padapter->securitypriv.dot11PrivacyAlgrthm == _TKIP_) ||
 	    (padapter->securitypriv.dot11PrivacyAlgrthm == _AES_)) {
 		/* restore group key, WEP keys is restored in ips_leave() */
-		rtw_set_key(
-			padapter,
-			psecuritypriv,
-			psecuritypriv->dot118021XGrpKeyid,
-			0,
-			false
-		);
+		rtw_set_key(padapter,
+			    psecuritypriv,
+			    psecuritypriv->dot118021XGrpKeyid,
+			    0,
+			    false);
 	}
 
 	spin_lock_bh(&pstapriv->asoc_list_lock);
@@ -2126,11 +2080,9 @@ void stop_ap_mode(struct adapter *padapter)
 	pmlmeext->bstart_bss = false;
 
 	/* reset and init security priv , this can refine with rtw_reset_securitypriv */
-	memset(
-		(unsigned char *)&padapter->securitypriv,
-		0,
-		sizeof(struct security_priv)
-	);
+	memset((unsigned char *)&padapter->securitypriv,
+	       0,
+	       sizeof(struct security_priv));
 	padapter->securitypriv.ndisauthtype = Ndis802_11AuthModeOpen;
 	padapter->securitypriv.ndisencryptstatus = Ndis802_11WEPDisabled;
 
diff --git a/drivers/staging/rtl8723bs/core/rtw_efuse.c b/drivers/staging/rtl8723bs/core/rtw_efuse.c
index d5c53b614f61..98b15ca10074 100644
--- a/drivers/staging/rtl8723bs/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723bs/core/rtw_efuse.c
@@ -26,9 +26,6 @@ u8 fakeBTEfuseContent[EFUSE_MAX_BT_BANK][EFUSE_MAX_HW_SIZE];
 u8 fakeBTEfuseInitMap[EFUSE_BT_MAX_MAP_LEN] = {0};
 u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN] = {0};
 
-#define REG_EFUSE_CTRL		0x0030
-#define EFUSE_CTRL			REG_EFUSE_CTRL		/*  E-Fuse Control. */
-
 /*  11/16/2008 MH Add description. Get current efuse area enabled word!!. */
 u8
 Efuse_CalculateWordCnts(u8 word_en)
diff --git a/drivers/staging/rtl8723bs/core/rtw_ieee80211.c b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
index 53d4c113b19c..8fdeeda88a6d 100644
--- a/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723bs/core/rtw_ieee80211.c
@@ -132,30 +132,30 @@ u8 *rtw_set_ie(u8 *pbuf,
 	return pbuf + len + 2;
 }
 
-/*----------------------------------------------------------------------------
-index: the information element id index, limit is the limit for search
------------------------------------------------------------------------------*/
+/* index: the information element id index, limit is the limit for search */
 u8 *rtw_get_ie(u8 *pbuf, signed int index, signed int *len, signed int limit)
 {
 	signed int tmp, i;
 	u8 *p;
 
-	if (limit < 1)
+	if (limit < 2)
 		return NULL;
 
 	p = pbuf;
 	i = 0;
 	*len = 0;
-	while (1) {
+	while (i + 2 <= limit) {
+		tmp = *(p + 1);
+		if (i + 2 + tmp > limit)
+			break;
+
 		if (*p == index) {
-			*len = *(p + 1);
+			*len = tmp;
 			return p;
 		}
-		tmp = *(p + 1);
+
 		p += (tmp + 2);
 		i += (tmp + 2);
-		if (i >= limit)
-			break;
 	}
 	return NULL;
 }
@@ -560,7 +560,6 @@ int rtw_parse_wpa2_ie(u8 *rsn_ie, int rsn_ie_len, int *group_cipher, int *pairwi
 	return ret;
 }
 
-/* ifdef CONFIG_WAPI_SUPPORT */
 int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
 {
 	int len = 0;
@@ -600,7 +599,6 @@ int rtw_get_wapi_ie(u8 *in_ie, uint in_len, u8 *wapi_ie, u16 *wapi_len)
 
 	return len;
 }
-/* endif */
 
 void rtw_get_sec_ie(u8 *in_ie, uint in_len, u8 *rsn_ie, u16 *rsn_len, u8 *wpa_ie, u16 *wpa_len)
 {
@@ -769,21 +767,27 @@ static int rtw_ieee802_11_parse_vendor_specific(u8 *pos, uint elen,
 {
 	unsigned int oui;
 
-	/* first 3 bytes in vendor specific information element are the IEEE
+	/*
+	 * first 3 bytes in vendor specific information element are the IEEE
 	 * OUI of the vendor. The following byte is used a vendor specific
-	 * sub-type. */
+	 * sub-type.
+	 */
 	if (elen < 4)
 		return -1;
 
 	oui = get_unaligned_be24(pos);
 	switch (oui) {
 	case OUI_MICROSOFT:
-		/* Microsoft/Wi-Fi information elements are further typed and
-		 * subtyped */
+		/*
+		 * Microsoft/Wi-Fi information elements are further typed and
+		 * subtyped
+		 */
 		switch (pos[3]) {
 		case 1:
-			/* Microsoft OUI (00:50:F2) with OUI Type 1:
-			 * real WPA information element */
+			/*
+			 * Microsoft OUI (00:50:F2) with OUI Type 1:
+			 * real WPA information element
+			 */
 			elems->wpa_ie = pos;
 			elems->wpa_ie_len = elen;
 			break;
diff --git a/drivers/staging/rtl8723bs/core/rtw_io.c b/drivers/staging/rtl8723bs/core/rtw_io.c
index 79d543d88278..fe9f94001eed 100644
--- a/drivers/staging/rtl8723bs/core/rtw_io.c
+++ b/drivers/staging/rtl8723bs/core/rtw_io.c
@@ -5,25 +5,23 @@
  *
  ******************************************************************************/
 /*
-
-The purpose of rtw_io.c
-
-a. provides the API
-
-b. provides the protocol engine
-
-c. provides the software interface between caller and the hardware interface
-
-
-Compiler Flag Option:
-
-1. CONFIG_SDIO_HCI:
-    a. USE_SYNC_IRP:  Only sync operations are provided.
-    b. USE_ASYNC_IRP:Both sync/async operations are provided.
-
-jackson@realtek.com.tw
-
-*/
+ * The purpose of rtw_io.c
+ *
+ * a. provides the API
+ *
+ * b. provides the protocol engine
+ *
+ * c. provides the software interface between caller and the hardware interface
+ *
+ *
+ * Compiler Flag Option:
+ *
+ * 1. CONFIG_SDIO_HCI:
+ *  a. USE_SYNC_IRP:  Only sync operations are provided.
+ *  b. USE_ASYNC_IRP:Both sync/async operations are provided.
+ *
+ *  jackson@realtek.com.tw
+ */
 
 #include <drv_types.h>
 
@@ -135,10 +133,10 @@ int rtw_init_io_priv(struct adapter *padapter, void (*set_intf_ops)(struct adapt
 }
 
 /*
-* Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR
-* @return true:
-* @return false:
-*/
+ * Increase and check if the continual_io_error of this @param dvobjprive is larger than MAX_CONTINUAL_IO_ERR
+ * @return true:
+ * @return false:
+ */
 int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj)
 {
 	int error_count = atomic_inc_return(&dvobj->continual_io_error);
@@ -149,9 +147,7 @@ int rtw_inc_and_chk_continual_io_error(struct dvobj_priv *dvobj)
 	return false;
 }
 
-/*
-* Set the continual_io_error of this @param dvobjprive to 0
-*/
+/* Set the continual_io_error of this @param dvobjprive to 0 */
 void rtw_reset_continual_io_error(struct dvobj_priv *dvobj)
 {
 	atomic_set(&dvobj->continual_io_error, 0);
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme.c b/drivers/staging/rtl8723bs/core/rtw_mlme.c
index c06d990350e6..98704179ad35 100644
--- a/drivers/staging/rtl8723bs/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme.c
@@ -214,10 +214,10 @@ void _rtw_free_network_nolock(struct	mlme_priv *pmlmepriv, struct wlan_network *
 }
 
 /*
-	return the wlan_network with the matching addr
-
-	Shall be called under atomic context... to avoid possible racing condition...
-*/
+ * return the wlan_network with the matching addr
+ *
+ * Shall be called under atomic context... to avoid possible racing condition...
+ */
 struct wlan_network *_rtw_find_network(struct __queue *scanned_queue, u8 *addr)
 {
 	struct list_head	*phead, *plist;
@@ -319,10 +319,10 @@ void rtw_free_network_nolock(struct adapter *padapter, struct wlan_network *pnet
 }
 
 /*
-	return the wlan_network with the matching addr
-
-	Shall be called under atomic context... to avoid possible racing condition...
-*/
+ * return the wlan_network with the matching addr
+ *
+ * Shall be called under atomic context... to avoid possible racing condition...
+ */
 struct	wlan_network *rtw_find_network(struct __queue *scanned_queue, u8 *addr)
 {
 	struct	wlan_network *pnetwork = _rtw_find_network(scanned_queue, addr);
@@ -476,9 +476,7 @@ static void update_current_network(struct adapter *adapter, struct wlan_bssid_ex
 	}
 }
 
-/*
-Caller must hold pmlmepriv->lock first.
-*/
+/* Caller must hold pmlmepriv->lock first. */
 void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *target)
 {
 	struct list_head	*plist, *phead;
@@ -510,8 +508,10 @@ void rtw_update_scanned_network(struct adapter *adapter, struct wlan_bssid_ex *t
 			oldest = pnetwork;
 	}
 
-	/* If we didn't find a match, then get a new network slot to initialize
-	 * with this beacon's information */
+	/*
+	 * If we didn't find a match, then get a new network slot to initialize
+	 * with this beacon's information
+	 */
 	if (!target_find) {
 		if (list_empty(&pmlmepriv->free_bss_pool.queue)) {
 			/* If there are no more slots, expire the oldest */
@@ -843,9 +843,7 @@ static void find_network(struct adapter *adapter)
 		rtw_free_network_nolock(adapter, pwlan);
 }
 
-/*
-*rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock
-*/
+/* rtw_free_assoc_resources: the caller has to lock pmlmepriv->lock */
 void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
 {
 	struct	mlme_priv *pmlmepriv = &adapter->mlmepriv;
@@ -879,9 +877,7 @@ void rtw_free_assoc_resources(struct adapter *adapter, int lock_scanned_queue)
 	rtw_reset_rx_info(pdbgpriv);
 }
 
-/*
-*rtw_indicate_connect: the caller has to lock pmlmepriv->lock
-*/
+/* rtw_indicate_connect: the caller has to lock pmlmepriv->lock */
 void rtw_indicate_connect(struct adapter *padapter)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -908,9 +904,7 @@ void rtw_indicate_connect(struct adapter *padapter)
 	rtw_set_scan_deny(padapter, 3000);
 }
 
-/*
-*rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock
-*/
+/* rtw_indicate_disconnect: the caller has to lock pmlmepriv->lock */
 void rtw_indicate_disconnect(struct adapter *padapter)
 {
 	struct	mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -1543,9 +1537,9 @@ void rtw_wmm_event_callback(struct adapter *padapter, u8 *pbuf)
 }
 
 /*
-* _rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss
-* @adapter: pointer to struct adapter structure
-*/
+ * _rtw_join_timeout_handler - Timeout/failure handler for CMD JoinBss
+ * @adapter: pointer to struct adapter structure
+ */
 void _rtw_join_timeout_handler(struct timer_list *t)
 {
 	struct adapter *adapter = timer_container_of(adapter, t,
@@ -1586,9 +1580,9 @@ void _rtw_join_timeout_handler(struct timer_list *t)
 }
 
 /*
-* rtw_scan_timeout_handler - Timeout/Failure handler for CMD SiteSurvey
-* @adapter: pointer to struct adapter structure
-*/
+ * rtw_scan_timeout_handler - Timeout/Failure handler for CMD SiteSurvey
+ * @adapter: pointer to struct adapter structure
+ */
 void rtw_scan_timeout_handler(struct timer_list *t)
 {
 	struct adapter *adapter = timer_container_of(adapter, t,
@@ -1704,10 +1698,10 @@ void rtw_set_scan_deny(struct adapter *adapter, u32 ms)
 }
 
 /*
-* Select a new roaming candidate from the original @param candidate and @param competitor
-* @return true: candidate is updated
-* @return false: candidate is not updated
-*/
+ * Select a new roaming candidate from the original @param candidate and @param competitor
+ * @return true: candidate is updated
+ * @return false: candidate is not updated
+ */
 static int rtw_check_roaming_candidate(struct mlme_priv *mlme
 	, struct wlan_network **candidate, struct wlan_network *competitor)
 {
@@ -1785,10 +1779,10 @@ exit:
 }
 
 /*
-* Select a new join candidate from the original @param candidate and @param competitor
-* @return true: candidate is updated
-* @return false: candidate is not updated
-*/
+ * Select a new join candidate from the original @param candidate and @param competitor
+ * @return true: candidate is updated
+ * @return false: candidate is not updated
+ */
 static int rtw_check_join_candidate(struct mlme_priv *mlme
 	, struct wlan_network **candidate, struct wlan_network *competitor)
 {
@@ -1829,11 +1823,11 @@ exit:
 }
 
 /*
-Calling context:
-The caller of the sub-routine will be in critical section...
-The caller must hold the following spinlock
-pmlmepriv->lock
-*/
+ * Calling context:
+ * The caller of the sub-routine will be in critical section...
+ * The caller must hold the following spinlock
+ * pmlmepriv->lock
+ */
 
 int rtw_select_and_join_from_scanned_queue(struct mlme_priv *pmlmepriv)
 {
diff --git a/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
index a897c433d2b0..ac49bfbaa5bb 100644
--- a/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723bs/core/rtw_mlme_ext.c
@@ -18,9 +18,7 @@ static struct mlme_handler mlme_sta_tbl[] = {
 	{WIFI_PROBEREQ,		"OnProbeReq",	&OnProbeReq},
 	{WIFI_PROBERSP,		"OnProbeRsp",		&OnProbeRsp},
 
-	/*----------------------------------------------------------
-					below 2 are reserved
-	-----------------------------------------------------------*/
+	/* below 2 are reserved */
 	{0,					"DoReserved",		&DoReserved},
 	{0,					"DoReserved",		&DoReserved},
 	{WIFI_BEACON,		"OnBeacon",		&OnBeacon},
@@ -50,9 +48,7 @@ static struct action_handler OnAction_tbl[] = {
 
 static u8 null_addr[ETH_ALEN] = {0, 0, 0, 0, 0, 0};
 
-/**************************************************
-OUI definitions for the vendor specific IE
-***************************************************/
+/* OUI definitions for the vendor specific IE */
 unsigned char RTW_WPA_OUI[] = {0x00, 0x50, 0xf2, 0x01};
 unsigned char WMM_OUI[] = {0x00, 0x50, 0xf2, 0x02};
 unsigned char WPS_OUI[] = {0x00, 0x50, 0xf2, 0x04};
@@ -64,9 +60,7 @@ unsigned char WMM_PARA_OUI[] = {0x00, 0x50, 0xf2, 0x02, 0x01, 0x01};
 
 static unsigned char REALTEK_96B_IE[] = {0x00, 0xe0, 0x4c, 0x02, 0x01, 0x20};
 
-/********************************************************
-ChannelPlan definitions
-*********************************************************/
+/* ChannelPlan definitions */
 static struct rt_channel_plan_2g	RTW_ChannelPlan2G[RT_CHANNEL_DOMAIN_2G_MAX] = {
 	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x00, RT_CHANNEL_DOMAIN_2G_WORLD , Passive scan CH 12, 13 */
 	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13}, 13},		/*  0x01, RT_CHANNEL_DOMAIN_2G_ETSI1 */
@@ -187,11 +181,7 @@ int rtw_ch_set_search_ch(struct rt_channel_info *ch_set, const u32 ch)
 	return i;
 }
 
-/****************************************************************************
-
-Following are the initialization functions for WiFi MLME
-
-*****************************************************************************/
+/* Following are the initialization functions for WiFi MLME */
 
 int init_hw_mlme_ext(struct adapter *padapter)
 {
@@ -507,11 +497,7 @@ void mgt_dispatcher(struct adapter *padapter, union recv_frame *precv_frame)
 	}
 }
 
-/****************************************************************************
-
-Following are the callback functions for each subtype of the management frames
-
-*****************************************************************************/
+/* Following are the callback functions for each subtype of the management frames */
 
 unsigned int OnProbeReq(struct adapter *padapter, union recv_frame *precv_frame)
 {
@@ -588,9 +574,11 @@ unsigned int OnBeacon(struct adapter *padapter, union recv_frame *precv_frame)
 
 	p = rtw_get_ie(pframe + sizeof(struct ieee80211_hdr_3addr) + _BEACON_IE_OFFSET_, WLAN_EID_EXT_SUPP_RATES, &ielen, precv_frame->u.hdr.len - sizeof(struct ieee80211_hdr_3addr) - _BEACON_IE_OFFSET_);
 	if (p && ielen > 0) {
-		if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
-			/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
-			*(p + 1) = ielen - 1;
+		if (p + 2 + ielen < pframe + len) {
+			if ((*(p + 1 + ielen) == 0x2D) && (*(p + 2 + ielen) != 0x2D))
+				/* Invalid value 0x2D is detected in Extended Supported Rates (ESR) IE. Try to fix the IE length to avoid failed Beacon parsing. */
+				*(p + 1) = ielen - 1;
+		}
 	}
 
 	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
@@ -1042,6 +1030,9 @@ unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
 		status = WLAN_STATUS_CHALLENGE_FAIL;
 		goto OnAssocReqFail;
 	} else {
+		if (ie_len > sizeof(supportRate))
+			ie_len = sizeof(supportRate);
+
 		memcpy(supportRate, p+2, ie_len);
 		supportRateNum = ie_len;
 
@@ -1049,7 +1040,7 @@ unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
 				pkt_len - WLAN_HDR_A3_LEN - ie_offset);
 		if (p) {
 
-			if (supportRateNum <= sizeof(supportRate)) {
+			if (supportRateNum + ie_len <= sizeof(supportRate)) {
 				memcpy(supportRate+supportRateNum, p+2, ie_len);
 				supportRateNum += ie_len;
 			}
@@ -1062,7 +1053,7 @@ unsigned int OnAssocReq(struct adapter *padapter, union recv_frame *precv_frame)
 	/* update station supportRate */
 	pstat->bssratelen = supportRateNum;
 	memcpy(pstat->bssrateset, supportRate, supportRateNum);
-	UpdateBrateTblForSoftAP(pstat->bssrateset, pstat->bssratelen);
+	update_basic_rate_table_soft_ap(pstat->bssrateset, pstat->bssratelen);
 
 	/* check RSN/WPA/WPS */
 	pstat->dot8021xalg = 0;
@@ -1450,7 +1441,7 @@ unsigned int OnAssocRsp(struct adapter *padapter, union recv_frame *precv_frame)
 	pmlmeinfo->state |= WIFI_FW_ASSOC_SUCCESS;
 
 	/* Update Basic Rate Table for spec, 2010-12-28 , by thomas */
-	UpdateBrateTbl(padapter, pmlmeinfo->network.supported_rates);
+	update_basic_rate_table(padapter, pmlmeinfo->network.supported_rates);
 
 report_assoc_result:
 	if (res > 0)
@@ -1950,11 +1941,7 @@ inline struct xmit_frame *alloc_mgtxmitframe(struct xmit_priv *pxmitpriv)
 	return _alloc_mgtxmitframe(pxmitpriv, false);
 }
 
-/****************************************************************************
-
-Following are some TX functions for WiFi MLME
-
-*****************************************************************************/
+/* Following are some TX functions for WiFi MLME */
 
 void update_mgnt_tx_rate(struct adapter *padapter, u8 rate)
 {
@@ -3797,11 +3784,7 @@ unsigned int send_beacon(struct adapter *padapter)
 		return _SUCCESS;
 }
 
-/****************************************************************************
-
-Following are some utility functions for WiFi MLME
-
-*****************************************************************************/
+/* Following are some utility functions for WiFi MLME */
 
 void site_survey(struct adapter *padapter)
 {
@@ -4392,11 +4375,7 @@ static void process_80211d(struct adapter *padapter, struct wlan_bssid_ex *bssid
 	}
 }
 
-/****************************************************************************
-
-Following are the functions to report events
-
-*****************************************************************************/
+/* Following are the functions to report events */
 
 void report_survey_event(struct adapter *padapter, union recv_frame *precv_frame)
 {
@@ -4692,11 +4671,7 @@ void report_add_sta_event(struct adapter *padapter, unsigned char *MacAddr, int
 	rtw_enqueue_cmd(pcmdpriv, pcmd_obj);
 }
 
-/****************************************************************************
-
-Following are the event callback functions
-
-*****************************************************************************/
+/* Following are the event callback functions */
 
 /* for sta/adhoc mode */
 void update_sta_info(struct adapter *padapter, struct sta_info *psta)
@@ -4863,8 +4838,10 @@ void mlmeext_joinbss_event_callback(struct adapter *padapter, int join_res)
 
 		rtw_sta_media_status_rpt(padapter, psta, 1);
 
-		/* wakeup macid after join bss successfully to ensure
-			the subsequent data frames can be sent out normally */
+		/*
+		 * wakeup macid after join bss successfully to ensure
+		 * the subsequent data frames can be sent out normally
+		 */
 		rtw_hal_macid_wakeup(padapter, psta->mac_id);
 	}
 
@@ -4940,11 +4917,8 @@ void mlmeext_sta_del_event_callback(struct adapter *padapter)
 		rtw_mlmeext_disconnect(padapter);
 }
 
-/****************************************************************************
-
-Following are the functions for the timer handlers
+/* Following are the functions for the timer handlers */
 
-*****************************************************************************/
 void _linked_info_dump(struct adapter *padapter)
 {
 	int i;
@@ -5275,7 +5249,7 @@ u8 createbss_hdl(struct adapter *padapter, u8 *pbuf)
 		/* clear CAM */
 		flush_all_cam_entry(padapter);
 
-		memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, ie_length));
+		memcpy(pnetwork, pbuf, offsetof(struct wlan_bssid_ex, ie_length));
 		pnetwork->ie_length = ((struct wlan_bssid_ex *)pbuf)->ie_length;
 
 		if (pnetwork->ie_length > MAX_IE_SZ)/* Check pbuf->ie_length */
@@ -5339,7 +5313,7 @@ u8 join_cmd_hdl(struct adapter *padapter, u8 *pbuf)
 	/* pmlmeinfo->assoc_AP_vendor = HT_IOT_PEER_MAX; */
 	pmlmeinfo->VHT_enable = 0;
 
-	memcpy(pnetwork, pbuf, FIELD_OFFSET(struct wlan_bssid_ex, ie_length));
+	memcpy(pnetwork, pbuf, offsetof(struct wlan_bssid_ex, ie_length));
 	pnetwork->ie_length = ((struct wlan_bssid_ex *)pbuf)->ie_length;
 
 	if (pnetwork->ie_length > MAX_IE_SZ)/* Check pbuf->ie_length */
diff --git a/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
index 7b643ac320f0..0ef788abf403 100644
--- a/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723bs/core/rtw_pwrctrl.c
@@ -999,11 +999,11 @@ inline void rtw_set_ips_deny(struct adapter *padapter, u32 ms)
 }
 
 /*
-* rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
-* @adapter: pointer to struct adapter structure
-* @ips_deffer_ms: the ms will prevent from falling into IPS after wakeup
-* Return _SUCCESS or _FAIL
-*/
+ * rtw_pwr_wakeup - Wake the NIC up from: 1)IPS. 2)USB autosuspend
+ * @adapter: pointer to struct adapter structure
+ * @ips_deffer_ms: the ms will prevent from falling into IPS after wakeup
+ * Return _SUCCESS or _FAIL
+ */
 
 int _rtw_pwr_wakeup(struct adapter *padapter, u32 ips_deffer_ms, const char *caller)
 {
diff --git a/drivers/staging/rtl8723bs/core/rtw_security.c b/drivers/staging/rtl8723bs/core/rtw_security.c
index 3d99d045f4b6..2f941ffbd465 100644
--- a/drivers/staging/rtl8723bs/core/rtw_security.c
+++ b/drivers/staging/rtl8723bs/core/rtw_security.c
@@ -30,9 +30,7 @@ const char *security_type_str(u8 value)
 
 /* WEP related ===== */
 
-/*
-	Need to consider the fragment  situation
-*/
+/* Need to consider the fragment  situation */
 void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
 {																	/*  exclude ICV */
 	union {
@@ -62,14 +60,14 @@ void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
 		keylength = psecuritypriv->dot11DefKeylen[psecuritypriv->dot11PrivacyKeyIndex];
 
 		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
-			iv = pframe+pattrib->hdrlen;
+			iv = pframe + pattrib->hdrlen;
 			memcpy(&wepkey[0], iv, 3);
 			memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength);
-			payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+			payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 
-			if ((curfragnum+1) == pattrib->nr_frags) {	/* the last fragment */
+			if ((curfragnum + 1) == pattrib->nr_frags) {	/* the last fragment */
 
-				length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+				length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 
 				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
 
@@ -78,7 +76,7 @@ void rtw_wep_encrypt(struct adapter *padapter, u8 *pxmitframe)
 				arc4_crypt(ctx, payload + length, crc.f1, 4);
 
 			} else {
-				length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+				length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 				crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
 				arc4_setkey(ctx, wepkey, 3 + keylength);
 				arc4_crypt(ctx, payload, payload, length);
@@ -107,16 +105,16 @@ void rtw_wep_decrypt(struct adapter  *padapter, u8 *precvframe)
 
 	/* start to decrypt recvframe */
 	if ((prxattrib->encrypt == _WEP40_) || (prxattrib->encrypt == _WEP104_)) {
-		iv = pframe+prxattrib->hdrlen;
+		iv = pframe + prxattrib->hdrlen;
 		/* keyindex =(iv[3]&0x3); */
 		keyindex = prxattrib->key_index;
 		keylength = psecuritypriv->dot11DefKeylen[keyindex];
 		memcpy(&wepkey[0], iv, 3);
 		/* memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[psecuritypriv->dot11PrivacyKeyIndex].skey[0], keylength); */
 		memcpy(&wepkey[3], &psecuritypriv->dot11DefKey[keyindex].skey[0], keylength);
-		length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+		length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
 
-		payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
+		payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
 
 		/* decrypt payload include icv */
 		arc4_setkey(ctx, wepkey, 3 + keylength);
@@ -174,7 +172,7 @@ void rtw_secmicsetkey(struct mic_data *pmicdata, u8 *key)
 void rtw_secmicappendbyte(struct mic_data *pmicdata, u8 b)
 {
 	/*  Append the byte to our word-sized buffer */
-	pmicdata->M |= ((unsigned long)b) << (8*pmicdata->nBytesInM);
+	pmicdata->M |= ((unsigned long)b) << (8 * pmicdata->nBytesInM);
 	pmicdata->nBytesInM++;
 	/*  Process the word if it is full. */
 	if (pmicdata->nBytesInM >= 4) {
@@ -261,7 +259,7 @@ void rtw_seccalctkipmic(u8 *key, u8 *header, u8 *data, u32 data_len, u8 *mic_cod
 #define  Mk16(hi, lo) ((lo) ^ (((u16)(hi)) << 8))
 
 /* select the Nth 16-bit word of the temporal key unsigned char array TK[]   */
-#define  TK16(N)     Mk16(tk[2*(N)+1], tk[2*(N)])
+#define  TK16(N)     Mk16(tk[2 * (N) + 1], tk[2 * (N)])
 
 /* S-box lookup: 16 bits --> 16 bits */
 #define _S_(v16)     (Sbox1[0][Lo8(v16)] ^ Sbox1[1][Hi8(v16)])
@@ -343,23 +341,20 @@ static const unsigned short Sbox1[2][256] = {      /* Sbox for hash (can be in R
 	}
 };
 
- /*
-**********************************************************************
-* Routine: Phase 1 -- generate P1K, given TA, TK, IV32
-*
-* Inputs:
-*     tk[]      = temporal key                         [128 bits]
-*     ta[]      = transmitter's MAC address            [ 48 bits]
-*     iv32      = upper 32 bits of IV                  [ 32 bits]
-* Output:
-*     p1k[]     = Phase 1 key                          [ 80 bits]
-*
-* Note:
-*     This function only needs to be called every 2**16 packets,
-*     although in theory it could be called every packet.
-*
-**********************************************************************
-*/
+/*
+ * Routine: Phase 1 -- generate P1K, given TA, TK, IV32
+ *
+ * Inputs:
+ *     tk[]      = temporal key                         [128 bits]
+ *     ta[]      = transmitter's MAC address            [ 48 bits]
+ *     iv32      = upper 32 bits of IV                  [ 32 bits]
+ * Output:
+ *     p1k[]     = Phase 1 key                          [ 80 bits]
+ *
+ * Note:
+ *     This function only needs to be called every 2**16 packets,
+ *     although in theory it could be called every packet.
+ */
 static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 {
 	signed int  i;
@@ -375,39 +370,36 @@ static void phase1(u16 *p1k, const u8 *tk, const u8 *ta, u32 iv32)
 	/* size on the 80-bit block P1K[], using the 128-bit key TK[] */
 	for (i = 0; i < PHASE1_LOOP_CNT; i++) {
 		/* Each add operation here is mod 2**16 */
-		p1k[0] += _S_(p1k[4] ^ TK16((i&1)+0));
-		p1k[1] += _S_(p1k[0] ^ TK16((i&1)+2));
-		p1k[2] += _S_(p1k[1] ^ TK16((i&1)+4));
-		p1k[3] += _S_(p1k[2] ^ TK16((i&1)+6));
-		p1k[4] += _S_(p1k[3] ^ TK16((i&1)+0));
+		p1k[0] += _S_(p1k[4] ^ TK16((i & 1) + 0));
+		p1k[1] += _S_(p1k[0] ^ TK16((i & 1) + 2));
+		p1k[2] += _S_(p1k[1] ^ TK16((i & 1) + 4));
+		p1k[3] += _S_(p1k[2] ^ TK16((i & 1) + 6));
+		p1k[4] += _S_(p1k[3] ^ TK16((i & 1) + 0));
 		p1k[4] +=  (unsigned short)i;          /* avoid "slide attacks" */
 	}
 }
 
 
 /*
-**********************************************************************
-* Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
-*
-* Inputs:
-*     tk[]      = Temporal key                         [128 bits]
-*     p1k[]     = Phase 1 output key                   [ 80 bits]
-*     iv16      = low 16 bits of IV counter            [ 16 bits]
-* Output:
-*     rc4key[]  = the key used to encrypt the packet   [128 bits]
-*
-* Note:
-*     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
-*     across all packets using the same key TK value. Then, for a
-*     given value of TK[], this TKIP48 construction guarantees that
-*     the final RC4KEY value is unique across all packets.
-*
-* Suggested implementation optimization: if PPK[] is "overlaid"
-*     appropriately on RC4KEY[], there is no need for the final
-*     for loop below that copies the PPK[] result into RC4KEY[].
-*
-**********************************************************************
-*/
+ * Routine: Phase 2 -- generate RC4KEY, given TK, P1K, IV16
+ *
+ * Inputs:
+ *     tk[]      = Temporal key                         [128 bits]
+ *     p1k[]     = Phase 1 output key                   [ 80 bits]
+ *     iv16      = low 16 bits of IV counter            [ 16 bits]
+ * Output:
+ *     rc4key[]  = the key used to encrypt the packet   [128 bits]
+ *
+ * Note:
+ *     The value {TA, IV32, IV16} for Phase1/Phase2 must be unique
+ *     across all packets using the same key TK value. Then, for a
+ *     given value of TK[], this TKIP48 construction guarantees that
+ *     the final RC4KEY value is unique across all packets.
+ *
+ * Suggested implementation optimization: if PPK[] is "overlaid"
+ *     appropriately on RC4KEY[], there is no need for the final
+ *     for loop below that copies the PPK[] result into RC4KEY[].
+ */
 static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 {
 	signed int  i;
@@ -417,7 +409,7 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 	for (i = 0; i < 5; i++)
 		PPK[i] = p1k[i];      /* first, copy P1K to PPK      */
 
-	PPK[5]  =  p1k[4]+iv16;             /* next,  add in IV16          */
+	PPK[5]  =  p1k[4] + iv16;             /* next,  add in IV16          */
 
 	/* Bijective non-linear mixing of the 96 bits of PPK[0..5]           */
 	PPK[0] +=    _S_(PPK[5] ^ TK16(0));   /* Mix key in each "round"     */
@@ -448,8 +440,8 @@ static void phase2(u8 *rc4key, const u8 *tk, const u16 *p1k, u16 iv16)
 
 	/* Copy 96 bits of PPK[0..5] to RC4KEY[4..15]  (little-endian)       */
 	for (i = 0; i < 6; i++) {
-		rc4key[4+2*i] = Lo8(PPK[i]);
-		rc4key[5+2*i] = Hi8(PPK[i]);
+		rc4key[4 + 2 * i] = Lo8(PPK[i]);
+		rc4key[5 + 2 * i] = Hi8(PPK[i]);
 	}
 }
 
@@ -492,20 +484,20 @@ u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
 				prwskey = pattrib->dot118021x_UncstKey.skey;
 
 			for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
-				iv = pframe+pattrib->hdrlen;
-				payload = pframe+pattrib->iv_len+pattrib->hdrlen;
+				iv = pframe + pattrib->hdrlen;
+				payload = pframe + pattrib->iv_len + pattrib->hdrlen;
 
 				GET_TKIP_PN(iv, dot11txpn);
 
 				pnl = (u16)(dot11txpn.val);
-				pnh = (u32)(dot11txpn.val>>16);
+				pnh = (u32)(dot11txpn.val >> 16);
 
 				phase1((u16 *)&ttkey[0], prwskey, &pattrib->ta[0], pnh);
 
 				phase2(&rc4key[0], prwskey, (u16 *)&ttkey[0], pnl);
 
-				if ((curfragnum+1) == pattrib->nr_frags) {	/* 4 the last fragment */
-					length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+				if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+					length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
 
 					arc4_setkey(ctx, rc4key, 16);
@@ -513,7 +505,7 @@ u32 rtw_tkip_encrypt(struct adapter *padapter, u8 *pxmitframe)
 					arc4_crypt(ctx, payload + length, crc.f1, 4);
 
 				} else {
-					length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+					length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 					crc.f0 = cpu_to_le32(~crc32_le(~0, payload, length));
 
 					arc4_setkey(ctx, rc4key, 16);
@@ -601,14 +593,14 @@ u32 rtw_tkip_decrypt(struct adapter *padapter, u8 *precvframe)
 				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 			}
 
-			iv = pframe+prxattrib->hdrlen;
-			payload = pframe+prxattrib->iv_len+prxattrib->hdrlen;
-			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+			iv = pframe + prxattrib->hdrlen;
+			payload = pframe + prxattrib->iv_len + prxattrib->hdrlen;
+			length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
 
 			GET_TKIP_PN(iv, dot11txpn);
 
 			pnl = (u16)(dot11txpn.val);
-			pnh = (u32)(dot11txpn.val>>16);
+			pnh = (u32)(dot11txpn.val >> 16);
 
 			phase1((u16 *)&ttkey[0], prwskey, &prxattrib->ta[0], pnh);
 			phase2(&rc4key[0], prwskey, (unsigned short *)&ttkey[0], pnl);
@@ -758,7 +750,7 @@ static void construct_mic_header2(u8 *mic_header2,
 
 		if (!qc_exists && a4_exists) {
 			for (i = 0; i < 6; i++)
-				mic_header2[8+i] = mpdu[24+i];   /* A4 */
+				mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
 		}
 
 		if (qc_exists && !a4_exists) {
@@ -768,7 +760,7 @@ static void construct_mic_header2(u8 *mic_header2,
 
 		if (qc_exists && a4_exists) {
 			for (i = 0; i < 6; i++)
-				mic_header2[8+i] = mpdu[24+i];   /* A4 */
+				mic_header2[8 + i] = mpdu[24 + i];   /* A4 */
 
 			mic_header2[14] = mpdu[30] & 0x0f;
 			mic_header2[15] = mpdu[31] & 0x00;
@@ -839,16 +831,16 @@ static signed int aes_cipher(u8 *key, uint	hdrlen,
 	uint	frtype  = GetFrameType(pframe);
 	uint	frsubtype  = GetFrameSubType(pframe);
 
-	frsubtype = frsubtype>>4;
+	frsubtype = frsubtype >> 4;
 
 	if ((hdrlen == WLAN_HDR_A3_LEN) || (hdrlen ==  WLAN_HDR_A3_QOS_LEN))
 		a4_exists = 0;
 	else
 		a4_exists = 1;
 
-	if (((frtype|frsubtype) == WIFI_DATA_CFACK) ||
-	    ((frtype|frsubtype) == WIFI_DATA_CFPOLL) ||
-	    ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) {
+	if (((frtype | frsubtype) == WIFI_DATA_CFACK) ||
+	    ((frtype | frsubtype) == WIFI_DATA_CFPOLL) ||
+	    ((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) {
 		qc_exists = 1;
 		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
 			hdrlen += 2;
@@ -867,11 +859,11 @@ static signed int aes_cipher(u8 *key, uint	hdrlen,
 	}
 
 	pn_vector[0] = pframe[hdrlen];
-	pn_vector[1] = pframe[hdrlen+1];
-	pn_vector[2] = pframe[hdrlen+4];
-	pn_vector[3] = pframe[hdrlen+5];
-	pn_vector[4] = pframe[hdrlen+6];
-	pn_vector[5] = pframe[hdrlen+7];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
 
 	construct_mic_iv(mic_iv,
 			 qc_exists,
@@ -927,12 +919,12 @@ static signed int aes_cipher(u8 *key, uint	hdrlen,
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-		pframe[payload_index+j] = mic[j];
+		pframe[payload_index + j] = mic[j];
 
 	payload_index = hdrlen + 8;
 	for (i = 0; i < num_blocks; i++) {
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, /* message, */
-				      pn_vector, i+1, frtype);
+				      pn_vector, i + 1, frtype);
 		/*  add for CONFIG_IEEE80211W, none 11w also can use */
 		aes128k128d(key, ctr_preload, aes_out);
 		crypto_xor_cpy(chain_buffer, aes_out, &pframe[payload_index], 16);
@@ -944,13 +936,13 @@ static signed int aes_cipher(u8 *key, uint	hdrlen,
 		/* If there is a short final block, then pad it,*/
 		/* encrypt it and copy the unpadded part back   */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, /* message, */
-				      pn_vector, num_blocks+1, frtype);
+				      pn_vector, num_blocks + 1, frtype);
 		/*  add for CONFIG_IEEE80211W, none 11w also can use */
 
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = pframe[payload_index+j];
+			padded_buffer[j] = pframe[payload_index + j];
 
 		aes128k128d(key, ctr_preload, aes_out);
 		crypto_xor_cpy(chain_buffer, aes_out, padded_buffer, 16);
@@ -966,7 +958,7 @@ static signed int aes_cipher(u8 *key, uint	hdrlen,
 	for (j = 0; j < 16; j++)
 		padded_buffer[j] = 0x00;
 	for (j = 0; j < 8; j++)
-		padded_buffer[j] = pframe[j+hdrlen+8+plen];
+		padded_buffer[j] = pframe[j + hdrlen + 8 + plen];
 
 	aes128k128d(key, ctr_preload, aes_out);
 	crypto_xor_cpy(chain_buffer, aes_out, padded_buffer, 16);
@@ -1006,12 +998,12 @@ u32 rtw_aes_encrypt(struct adapter *padapter, u8 *pxmitframe)
 			prwskey = pattrib->dot118021x_UncstKey.skey;
 
 		for (curfragnum = 0; curfragnum < pattrib->nr_frags; curfragnum++) {
-			if ((curfragnum+1) == pattrib->nr_frags) {	/* 4 the last fragment */
-				length = pattrib->last_txcmdsz-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+			if ((curfragnum + 1) == pattrib->nr_frags) {	/* 4 the last fragment */
+				length = pattrib->last_txcmdsz - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 
 				aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
 			} else {
-				length = pxmitpriv->frag_len-pattrib->hdrlen-pattrib->iv_len-pattrib->icv_len;
+				length = pxmitpriv->frag_len - pattrib->hdrlen - pattrib->iv_len - pattrib->icv_len;
 
 				aes_cipher(prwskey, pattrib->hdrlen, pframe, length);
 				pframe += pxmitpriv->frag_len;
@@ -1044,13 +1036,13 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 	uint frtype  = GetFrameType(pframe);
 	uint frsubtype  = GetFrameSubType(pframe);
 
-	frsubtype = frsubtype>>4;
+	frsubtype = frsubtype >> 4;
 
 	/* start to decrypt the payload */
 
-	num_blocks = (plen-8) / 16; /* plen including LLC, payload_length and mic) */
+	num_blocks = (plen - 8) / 16; /* plen including LLC, payload_length and mic) */
 
-	payload_remainder = (plen-8) % 16;
+	payload_remainder = (plen - 8) % 16;
 
 	pn_vector[0]  = pframe[hdrlen];
 	pn_vector[1]  = pframe[hdrlen + 1];
@@ -1064,9 +1056,9 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 	else
 		a4_exists = 1;
 
-	if (((frtype|frsubtype) == WIFI_DATA_CFACK) ||
-	    ((frtype|frsubtype) == WIFI_DATA_CFPOLL) ||
-	    ((frtype|frsubtype) == WIFI_DATA_CFACKPOLL)) {
+	if (((frtype | frsubtype) == WIFI_DATA_CFACK) ||
+	    ((frtype | frsubtype) == WIFI_DATA_CFPOLL) ||
+	    ((frtype | frsubtype) == WIFI_DATA_CFACKPOLL)) {
 		qc_exists = 1;
 		if (hdrlen !=  WLAN_HDR_A3_QOS_LEN)
 			hdrlen += 2;
@@ -1105,13 +1097,13 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 		/* If there is a short final block, then pad it,*/
 		/* encrypt it and copy the unpadded part back   */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, pframe, pn_vector,
-				      num_blocks+1, frtype);
+				      num_blocks + 1, frtype);
 		/*  add for CONFIG_IEEE80211W, none 11w also can use */
 
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = pframe[payload_index+j];
+			padded_buffer[j] = pframe[payload_index + j];
 
 		aes128k128d(key, ctr_preload, aes_out);
 		crypto_xor_cpy(chain_buffer, aes_out, padded_buffer, 16);
@@ -1120,25 +1112,25 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 	}
 
 	/* start to calculate the mic */
-	if ((hdrlen + plen+8) <= MAX_MSG_SIZE)
-		memcpy((void *)message, pframe, (hdrlen + plen+8)); /* 8 is for ext iv len */
+	if ((hdrlen + plen + 8) <= MAX_MSG_SIZE)
+		memcpy((void *)message, pframe, (hdrlen + plen + 8)); /* 8 is for ext iv len */
 
 	pn_vector[0] = pframe[hdrlen];
-	pn_vector[1] = pframe[hdrlen+1];
-	pn_vector[2] = pframe[hdrlen+4];
-	pn_vector[3] = pframe[hdrlen+5];
-	pn_vector[4] = pframe[hdrlen+6];
-	pn_vector[5] = pframe[hdrlen+7];
+	pn_vector[1] = pframe[hdrlen + 1];
+	pn_vector[2] = pframe[hdrlen + 4];
+	pn_vector[3] = pframe[hdrlen + 5];
+	pn_vector[4] = pframe[hdrlen + 6];
+	pn_vector[5] = pframe[hdrlen + 7];
 
-	construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen-8, pn_vector, frtype);
+	construct_mic_iv(mic_iv, qc_exists, a4_exists, message, plen - 8, pn_vector, frtype);
 	/*  add for CONFIG_IEEE80211W, none 11w also can use */
 
 	construct_mic_header1(mic_header1, hdrlen, message, frtype);
 	/*  add for CONFIG_IEEE80211W, none 11w also can use */
 	construct_mic_header2(mic_header2, message, a4_exists, qc_exists);
 
-	payload_remainder = (plen-8) % 16;
-	num_blocks = (plen-8) / 16;
+	payload_remainder = (plen - 8) % 16;
+	num_blocks = (plen - 8) / 16;
 
 	/* Find start of payload */
 	payload_index = (hdrlen + 8);
@@ -1173,11 +1165,11 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 
 	/* Insert MIC into payload */
 	for (j = 0; j < 8; j++)
-		message[payload_index+j] = mic[j];
+		message[payload_index + j] = mic[j];
 
 	payload_index = hdrlen + 8;
 	for (i = 0; i < num_blocks; i++) {
-		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i+1,
+		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector, i + 1,
 				      frtype);
 		/*  add for CONFIG_IEEE80211W, none 11w also can use */
 		aes128k128d(key, ctr_preload, aes_out);
@@ -1190,13 +1182,13 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 		/* If there is a short final block, then pad it,*/
 		/* encrypt it and copy the unpadded part back   */
 		construct_ctr_preload(ctr_preload, a4_exists, qc_exists, message, pn_vector,
-				      num_blocks+1, frtype);
+				      num_blocks + 1, frtype);
 		/*  add for CONFIG_IEEE80211W, none 11w also can use */
 
 		for (j = 0; j < 16; j++)
 			padded_buffer[j] = 0x00;
 		for (j = 0; j < payload_remainder; j++)
-			padded_buffer[j] = message[payload_index+j];
+			padded_buffer[j] = message[payload_index + j];
 
 		aes128k128d(key, ctr_preload, aes_out);
 		crypto_xor_cpy(chain_buffer, aes_out, padded_buffer, 16);
@@ -1211,7 +1203,7 @@ static signed int aes_decipher(u8 *key, uint	hdrlen,
 	for (j = 0; j < 16; j++)
 		padded_buffer[j] = 0x00;
 	for (j = 0; j < 8; j++)
-		padded_buffer[j] = message[j+hdrlen+8+plen-8];
+		padded_buffer[j] = message[j + hdrlen + 8 + plen - 8];
 
 	aes128k128d(key, ctr_preload, aes_out);
 	crypto_xor_cpy(chain_buffer, aes_out, padded_buffer, 16);
@@ -1298,7 +1290,7 @@ u32 rtw_aes_decrypt(struct adapter *padapter, u8 *precvframe)
 				prwskey = &stainfo->dot118021x_UncstKey.skey[0];
 			}
 
-			length = ((union recv_frame *)precvframe)->u.hdr.len-prxattrib->hdrlen-prxattrib->iv_len;
+			length = ((union recv_frame *)precvframe)->u.hdr.len - prxattrib->hdrlen - prxattrib->iv_len;
 
 			res = aes_decipher(prwskey, prxattrib->hdrlen, pframe, length);
 
@@ -1323,7 +1315,7 @@ u32 rtw_BIP_verify(struct adapter *padapter, u8 *precvframe)
 	__le16 le_tmp;
 	__le64 le_tmp64;
 
-	ori_len = pattrib->pkt_len-WLAN_HDR_A3_LEN+BIP_AAD_SIZE;
+	ori_len = pattrib->pkt_len - WLAN_HDR_A3_LEN + BIP_AAD_SIZE;
 	BIP_AAD = rtw_zmalloc(ori_len);
 
 	if (!BIP_AAD)
@@ -1334,28 +1326,28 @@ u32 rtw_BIP_verify(struct adapter *padapter, u8 *precvframe)
 	/* mapping to wlan header */
 	pwlanhdr = (struct ieee80211_hdr *)pframe;
 	/* save the frame body + MME */
-	memcpy(BIP_AAD+BIP_AAD_SIZE, pframe+WLAN_HDR_A3_LEN, pattrib->pkt_len-WLAN_HDR_A3_LEN);
+	memcpy(BIP_AAD + BIP_AAD_SIZE, pframe + WLAN_HDR_A3_LEN, pattrib->pkt_len - WLAN_HDR_A3_LEN);
 	/* find MME IE pointer */
-	p = rtw_get_ie(BIP_AAD+BIP_AAD_SIZE, WLAN_EID_MMIE, &len, pattrib->pkt_len-WLAN_HDR_A3_LEN);
+	p = rtw_get_ie(BIP_AAD + BIP_AAD_SIZE, WLAN_EID_MMIE, &len, pattrib->pkt_len - WLAN_HDR_A3_LEN);
 	/* Baron */
 	if (p) {
 		u16 keyid = 0;
 		u64 temp_ipn = 0;
 		/* save packet number */
-		memcpy(&le_tmp64, p+4, 6);
+		memcpy(&le_tmp64, p + 4, 6);
 		temp_ipn = le64_to_cpu(le_tmp64);
 		/* BIP packet number should bigger than previous BIP packet */
 		if (temp_ipn <= pmlmeext->mgnt_80211w_IPN_rx)
 			goto BIP_exit;
 
 		/* copy key index */
-		memcpy(&le_tmp, p+2, 2);
+		memcpy(&le_tmp, p + 2, 2);
 		keyid = le16_to_cpu(le_tmp);
 		if (keyid != padapter->securitypriv.dot11wBIPKeyid)
 			goto BIP_exit;
 
 		/* clear the MIC field of MME to zero */
-		memset(p+2+len-8, 0, 8);
+		memset(p + 2 + len - 8, 0, 8);
 
 		/* conscruct AAD, copy frame control field */
 		memcpy(BIP_AAD, &pwlanhdr->frame_control, 2);
@@ -1483,7 +1475,8 @@ static int omac1_aes_128_vector(u8 *key, size_t num_elem,
  * This is a mode for using block cipher (AES in this case) for authentication.
  * OMAC1 was standardized with the name CMAC by NIST in a Special Publication
  * (SP) 800-38B.
- * modify for CONFIG_IEEE80211W */
+ * modify for CONFIG_IEEE80211W
+ */
 int omac1_aes_128(u8 *key, u8 *data, size_t data_len, u8 *mac)
 {
 	return omac1_aes_128_vector(key, 1, &data, &data_len, mac);
@@ -1515,7 +1508,7 @@ u8 rtw_handle_tkip_countermeasure(struct adapter *adapter, const char *caller)
 	if (securitypriv->btkip_countermeasure) {
 		unsigned long passing_ms = jiffies_to_msecs(jiffies - securitypriv->btkip_countermeasure_time);
 
-		if (passing_ms > 60*1000) {
+		if (passing_ms > 60 * 1000) {
 			netdev_dbg(adapter->pnetdev,
 				   "%s(%s) countermeasure time:%lus > 60s\n",
 				   caller, ADPT_ARG(adapter),
diff --git a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
index d1f6030799cb..3e80d03c4ec9 100644
--- a/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
+++ b/drivers/staging/rtl8723bs/core/rtw_sta_mgt.c
@@ -383,12 +383,6 @@ u32 rtw_free_stainfo(struct adapter *padapter, struct sta_info *psta)
 
 	/* release mac id for non-bc/mc station, */
 	rtw_release_macid(pstapriv->padapter, psta);
-
-/*
-	spin_lock_bh(&pstapriv->asoc_list_lock);
-	list_del_init(&psta->asoc_list);
-	spin_unlock_bh(&pstapriv->asoc_list_lock);
-*/
 	spin_lock_bh(&pstapriv->auth_list_lock);
 	if (!list_empty(&psta->auth_list)) {
 		list_del_init(&psta->auth_list);
diff --git a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
index 1def9758852c..5ffefa50699e 100644
--- a/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723bs/core/rtw_wlan_util.c
@@ -181,7 +181,7 @@ void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask)
 	mcs_set[3] &= mcs_rate_4r;
 }
 
-void UpdateBrateTbl(struct adapter *Adapter, u8 *mBratesOS)
+void update_basic_rate_table(struct adapter *Adapter, u8 *mBratesOS)
 {
 	u8 i;
 	u8 rate;
@@ -203,7 +203,7 @@ void UpdateBrateTbl(struct adapter *Adapter, u8 *mBratesOS)
 	}
 }
 
-void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen)
+void update_basic_rate_table_soft_ap(u8 *bssrateset, u32 bssratelen)
 {
 	u8 i;
 	u8 rate;
@@ -1021,9 +1021,9 @@ void HTOnAssocRsp(struct adapter *padapter)
 
 	/* handle A-MPDU parameter field */
 	/*
-		AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
-		AMPDU_para [4:2]:Min MPDU Start Spacing
-	*/
+	 * AMPDU_para [1:0]:Max AMPDU Len => 0:8k , 1:16k, 2:32k, 3:64k
+	 * AMPDU_para [4:2]:Min MPDU Start Spacing
+	 */
 	max_AMPDU_len = pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x03;
 
 	min_MPDU_spacing = (pmlmeinfo->HT_caps.u.HT_cap_element.AMPDU_para & 0x1c) >> 2;
@@ -1689,15 +1689,6 @@ void adaptive_early_32k(struct mlme_ext_priv *pmlmeext, u8 *pframe, uint len)
 	else
 		pmlmeext->bcn_delay_cnt[delay_ms]++;
 		/* pmlmeext->bcn_delay_ratio[delay_ms] = (pmlmeext->bcn_delay_cnt[delay_ms] * 100) /pmlmeext->bcn_cnt; */
-
-/*
-
-	for (i = 0; i<9; i++)
-	{
-			pmlmeext->bcn_delay_cnt[i] , i, pmlmeext->bcn_delay_ratio[i]);
-	}
-*/
-
 	/* dump for  adaptive_early_32k */
 	if (pmlmeext->bcn_cnt > 100 && (pmlmeext->adaptive_tsf_done == true)) {
 		u8 ratio_20_delay, ratio_80_delay;
diff --git a/drivers/staging/rtl8723bs/hal/hal_com.c b/drivers/staging/rtl8723bs/hal/hal_com.c
index 07e9d3423651..70b5b289f9cb 100644
--- a/drivers/staging/rtl8723bs/hal/hal_com.c
+++ b/drivers/staging/rtl8723bs/hal/hal_com.c
@@ -663,71 +663,6 @@ void GetHwReg(struct adapter *adapter, u8 variable, u8 *val)
 	}
 }
 
-
-
-
-u8 SetHalDefVar(
-	struct adapter *adapter, enum hal_def_variable variable, void *value
-)
-{
-	struct hal_com_data *hal_data = GET_HAL_DATA(adapter);
-	struct dm_odm_t *odm = &(hal_data->odmpriv);
-	u8 bResult = _SUCCESS;
-
-	switch (variable) {
-	case HW_DEF_ODM_DBG_FLAG:
-		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_COMP, *((u64 *)value));
-		break;
-	case HW_DEF_ODM_DBG_LEVEL:
-		ODM_CmnInfoUpdate(odm, ODM_CMNINFO_DBG_LEVEL, *((u32 *)value));
-		break;
-	case HAL_DEF_DBG_DM_FUNC:
-	{
-		u8 dm_func = *((u8 *)value);
-		struct dm_priv *dm = &hal_data->dmpriv;
-
-		if (dm_func == 0) { /* disable all dynamic func */
-			odm->SupportAbility = DYNAMIC_FUNC_DISABLE;
-		} else if (dm_func == 1) {/* disable DIG */
-			odm->SupportAbility  &= (~DYNAMIC_BB_DIG);
-		} else if (dm_func == 2) {/* disable High power */
-			odm->SupportAbility  &= (~DYNAMIC_BB_DYNAMIC_TXPWR);
-		} else if (dm_func == 3) {/* disable tx power tracking */
-			odm->SupportAbility  &= (~DYNAMIC_RF_CALIBRATION);
-		} else if (dm_func == 4) {/* disable BT coexistence */
-			dm->DMFlag &= (~DYNAMIC_FUNC_BT);
-		} else if (dm_func == 5) {/* disable antenna diversity */
-			odm->SupportAbility  &= (~DYNAMIC_BB_ANT_DIV);
-		} else if (dm_func == 6) {/* turn on all dynamic func */
-			if (!(odm->SupportAbility  & DYNAMIC_BB_DIG)) {
-				struct dig_t	*pDigTable = &odm->DM_DigTable;
-				pDigTable->CurIGValue = rtw_read8(adapter, 0xc50);
-			}
-			dm->DMFlag |= DYNAMIC_FUNC_BT;
-			odm->SupportAbility = DYNAMIC_ALL_FUNC_ENABLE;
-		}
-	}
-		break;
-	case HAL_DEF_DBG_DUMP_RXPKT:
-		hal_data->bDumpRxPkt = *((u8 *)value);
-		break;
-	case HAL_DEF_DBG_DUMP_TXPKT:
-		hal_data->bDumpTxPkt = *((u8 *)value);
-		break;
-	case HAL_DEF_ANT_DETECT:
-		hal_data->AntDetection = *((u8 *)value);
-		break;
-	default:
-		netdev_dbg(adapter->pnetdev,
-			   "%s: [WARNING] HAL_DEF_VARIABLE(%d) not defined!\n",
-			   __func__, variable);
-		bResult = _FAIL;
-		break;
-	}
-
-	return bResult;
-}
-
 u8 GetHalDefVar(
 	struct adapter *adapter, enum hal_def_variable variable, void *value
 )
diff --git a/drivers/staging/rtl8723bs/hal/hal_intf.c b/drivers/staging/rtl8723bs/hal/hal_intf.c
index 961b0563951d..462553d296ff 100644
--- a/drivers/staging/rtl8723bs/hal/hal_intf.c
+++ b/drivers/staging/rtl8723bs/hal/hal_intf.c
@@ -115,11 +115,6 @@ void rtw_hal_set_hwreg_with_buf(struct adapter *padapter, u8 variable, u8 *pbuf,
 	SetHwRegWithBuf8723B(padapter, variable, pbuf, len);
 }
 
-u8 rtw_hal_set_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue)
-{
-	return SetHalDefVar8723BSDIO(padapter, eVariable, pValue);
-}
-
 u8 rtw_hal_get_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue)
 {
 	return GetHalDefVar8723BSDIO(padapter, eVariable, pValue);
diff --git a/drivers/staging/rtl8723bs/hal/odm.c b/drivers/staging/rtl8723bs/hal/odm.c
index 4b36af47f680..639b6da2302b 100644
--- a/drivers/staging/rtl8723bs/hal/odm.c
+++ b/drivers/staging/rtl8723bs/hal/odm.c
@@ -609,15 +609,12 @@ void ODM_DMWatchdog(struct dm_odm_t *pDM_Odm)
 	/* 8723A or 8189ES platform */
 	/* NeilChen--2012--08--24-- */
 	/* Fix Leave LPS issue */
-	if ((adapter_to_pwrctl(pDM_Odm->Adapter)->pwr_mode != PS_MODE_ACTIVE) /*  in LPS mode */
-		/*  */
-		/* (pDM_Odm->SupportICType & (ODM_RTL8723A))|| */
-		/* (pDM_Odm->SupportICType & (ODM_RTL8188E) &&(&&(((pDM_Odm->SupportInterface  == ODM_ITRF_SDIO))) */
-		/*  */
-	) {
-			odm_DIGbyRSSI_LPS(pDM_Odm);
-	} else
+	if (adapter_to_pwrctl(pDM_Odm->Adapter)->pwr_mode != PS_MODE_ACTIVE) {
+		/* in LPS mode */
+		odm_DIGbyRSSI_LPS(pDM_Odm);
+	} else {
 		odm_DIG(pDM_Odm);
+	}
 
 	{
 		struct dig_t *pDM_DigTable = &pDM_Odm->DM_DigTable;
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c b/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
index 63c4ebe9df12..af6cdda8238d 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_cmd.c
@@ -7,6 +7,7 @@
 
 #include <drv_types.h>
 #include <rtl8723b_hal.h>
+#include <linux/etherdevice.h>
 #include "hal_com_h2c.h"
 
 #define MAX_H2C_BOX_NUMS	4
@@ -117,8 +118,8 @@ static void ConstructBeacon(struct adapter *padapter, u8 *pframe, u32 *pLength)
 	*(fctrl) = 0;
 
 	eth_broadcast_addr(pwlanhdr->addr1);
-	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, get_my_bssid(cur_network), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)));
+	ether_addr_copy(pwlanhdr->addr3, get_my_bssid(cur_network));
 
 	SetSeqNum(pwlanhdr, 0/*pmlmeext->mgnt_seq*/);
 	/* pmlmeext->mgnt_seq++; */
@@ -209,10 +210,10 @@ static void ConstructPSPoll(struct adapter *padapter, u8 *pframe, u32 *pLength)
 	SetDuration(pframe, (pmlmeinfo->aid | 0xc000));
 
 	/*  BSSID. */
-	memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)));
 
 	/*  TA. */
-	memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)));
 
 	*pLength = 16;
 }
@@ -246,21 +247,21 @@ static void ConstructNullFunctionData(
 	switch (cur_network->network.infrastructure_mode) {
 	case Ndis802_11Infrastructure:
 		SetToDs(fctrl);
-		memcpy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
-		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
-		memcpy(pwlanhdr->addr3, StaAddr, ETH_ALEN);
+		ether_addr_copy(pwlanhdr->addr1, get_my_bssid(&(pmlmeinfo->network)));
+		ether_addr_copy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)));
+		ether_addr_copy(pwlanhdr->addr3, StaAddr);
 		break;
 	case Ndis802_11APMode:
 		SetFrDs(fctrl);
-		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
-		memcpy(pwlanhdr->addr2, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
-		memcpy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)), ETH_ALEN);
+		ether_addr_copy(pwlanhdr->addr1, StaAddr);
+		ether_addr_copy(pwlanhdr->addr2, get_my_bssid(&(pmlmeinfo->network)));
+		ether_addr_copy(pwlanhdr->addr3, myid(&(padapter->eeprompriv)));
 		break;
 	case Ndis802_11IBSS:
 	default:
-		memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
-		memcpy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)), ETH_ALEN);
-		memcpy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)), ETH_ALEN);
+		ether_addr_copy(pwlanhdr->addr1, StaAddr);
+		ether_addr_copy(pwlanhdr->addr2, myid(&(padapter->eeprompriv)));
+		ether_addr_copy(pwlanhdr->addr3, get_my_bssid(&(pmlmeinfo->network)));
 		break;
 	}
 
@@ -765,9 +766,9 @@ static void ConstructBtNullFunctionData(
 		SetPwrMgt(fctrl);
 
 	SetFrDs(fctrl);
-	memcpy(pwlanhdr->addr1, StaAddr, ETH_ALEN);
-	memcpy(pwlanhdr->addr2, myid(&padapter->eeprompriv), ETH_ALEN);
-	memcpy(pwlanhdr->addr3, myid(&padapter->eeprompriv), ETH_ALEN);
+	ether_addr_copy(pwlanhdr->addr1, StaAddr);
+	ether_addr_copy(pwlanhdr->addr2, myid(&padapter->eeprompriv));
+	ether_addr_copy(pwlanhdr->addr3, myid(&padapter->eeprompriv));
 
 	SetDuration(pwlanhdr, 0);
 	SetSeqNum(pwlanhdr, 0);
diff --git a/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
index 18244adad9e0..57c83f332e74 100644
--- a/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
+++ b/drivers/staging/rtl8723bs/hal/rtl8723b_hal_init.c
@@ -2840,22 +2840,6 @@ void GetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val)
 }
 
 /* Description:
- *	Change default setting of specified variable.
- */
-u8 SetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable, void *pval)
-{
-	u8 bResult = _SUCCESS;
-
-	switch (variable) {
-	default:
-		bResult = SetHalDefVar(padapter, variable, pval);
-		break;
-	}
-
-	return bResult;
-}
-
-/* Description:
  *	Query setting of specified variable.
  */
 u8 GetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable, void *pval)
diff --git a/drivers/staging/rtl8723bs/hal/sdio_halinit.c b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
index 7fcb874d0eb3..4e81ef53dc47 100644
--- a/drivers/staging/rtl8723bs/hal/sdio_halinit.c
+++ b/drivers/staging/rtl8723bs/hal/sdio_halinit.c
@@ -1014,14 +1014,10 @@ static void Hal_EfuseParseMACAddr_8723BS(
 	struct adapter *padapter, u8 *hwinfo, bool AutoLoadFail
 )
 {
-	u16 i;
-	u8 sMacAddr[6] = {0x00, 0xE0, 0x4C, 0xb7, 0x23, 0x00};
 	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(padapter);
 
 	if (AutoLoadFail) {
-/* 		sMacAddr[5] = (u8)GetRandomNumber(1, 254); */
-		for (i = 0; i < 6; i++)
-			pEEPROM->mac_addr[i] = sMacAddr[i];
+		eth_random_addr(pEEPROM->mac_addr);
 	} else {
 		/* Read Permanent MAC address */
 		memcpy(pEEPROM->mac_addr, &hwinfo[EEPROM_MAC_ADDR_8723BS], ETH_ALEN);
@@ -1236,12 +1232,3 @@ u8 GetHalDefVar8723BSDIO(
 
 	return bResult;
 }
-
-/*  */
-/* 	Description: */
-/* 		Change default setting of specified variable. */
-/*  */
-u8 SetHalDefVar8723BSDIO(struct adapter *Adapter, enum hal_def_variable eVariable, void *pValue)
-{
-	return SetHalDefVar8723B(Adapter, eVariable, pValue);
-}
diff --git a/drivers/staging/rtl8723bs/hal/sdio_ops.c b/drivers/staging/rtl8723bs/hal/sdio_ops.c
index 8736c124f857..0ee50b4a1149 100644
--- a/drivers/staging/rtl8723bs/hal/sdio_ops.c
+++ b/drivers/staging/rtl8723bs/hal/sdio_ops.c
@@ -997,10 +997,7 @@ u8 HalQueryTxBufferStatus8723BSdio(struct adapter *adapter)
 	return true;
 }
 
-/*  */
-/*	Description: */
-/*		Query SDIO Local register to get the current number of TX OQT Free Space. */
-/*  */
+/* Read the TX OQT free page count from the SDIO local register. */
 void HalQueryTxOQTBufferStatus8723BSdio(struct adapter *adapter)
 {
 	struct hal_com_data *haldata = GET_HAL_DATA(adapter);
diff --git a/drivers/staging/rtl8723bs/include/basic_types.h b/drivers/staging/rtl8723bs/include/basic_types.h
index 1c2da18e6210..8adb95f9f1e5 100644
--- a/drivers/staging/rtl8723bs/include/basic_types.h
+++ b/drivers/staging/rtl8723bs/include/basic_types.h
@@ -12,8 +12,7 @@
 #define FAIL	(-1)
 
 #include <linux/types.h>
-
-#define FIELD_OFFSET(s, field)	((__kernel_ssize_t)&((s *)(0))->field)
+#include <linux/stddef.h>
 
 #define SIZE_PTR __kernel_size_t
 #define SSIZE_PTR __kernel_ssize_t
diff --git a/drivers/staging/rtl8723bs/include/drv_types.h b/drivers/staging/rtl8723bs/include/drv_types.h
index dd9018aa4ee5..f86180dc350c 100644
--- a/drivers/staging/rtl8723bs/include/drv_types.h
+++ b/drivers/staging/rtl8723bs/include/drv_types.h
@@ -171,13 +171,6 @@ struct registry_priv {
 	u8 hiq_filter;
 };
 
-
-/* For registry parameters */
-#define RGTRY_OFT(field) ((u32)FIELD_OFFSET(struct registry_priv, field))
-#define RGTRY_SZ(field)   sizeof(((struct registry_priv *)0)->field)
-#define BSSID_OFT(field) ((u32)FIELD_OFFSET(struct wlan_bssid_ex, field))
-#define BSSID_SZ(field)   sizeof(((struct wlan_bssid_ex *) 0)->field)
-
 #include <drv_types_sdio.h>
 
 #define GET_PRIMARY_ADAPTER(padapter) (((struct adapter *)padapter)->dvobj->if1)
diff --git a/drivers/staging/rtl8723bs/include/hal_com.h b/drivers/staging/rtl8723bs/include/hal_com.h
index 7ea9ee2b3975..74d6c892c401 100644
--- a/drivers/staging/rtl8723bs/include/hal_com.h
+++ b/drivers/staging/rtl8723bs/include/hal_com.h
@@ -138,8 +138,6 @@ void SetHwReg(struct adapter *padapter, u8 variable, u8 *val);
 void GetHwReg(struct adapter *padapter, u8 variable, u8 *val);
 void rtw_hal_check_rxfifo_full(struct adapter *adapter);
 
-u8 SetHalDefVar(struct adapter *adapter, enum hal_def_variable variable,
-		void *value);
 u8 GetHalDefVar(struct adapter *adapter, enum hal_def_variable variable,
 		void *value);
 
diff --git a/drivers/staging/rtl8723bs/include/hal_com_reg.h b/drivers/staging/rtl8723bs/include/hal_com_reg.h
index 9a02ae69d7a4..cf5c15dc2bfd 100644
--- a/drivers/staging/rtl8723bs/include/hal_com_reg.h
+++ b/drivers/staging/rtl8723bs/include/hal_com_reg.h
@@ -189,10 +189,6 @@
 /* 	Redifine 8192C register definition for compatibility */
 /*  */
 /*  */
-
-/*  TODO: use these definition when using REG_xxx naming rule. */
-/*  NOTE: DO NOT Remove these definition. Use later. */
-
 #define EFUSE_CTRL				REG_EFUSE_CTRL		/*  E-Fuse Control. */
 #define EFUSE_TEST				REG_EFUSE_TEST		/*  E-Fuse Test. */
 #define MSR						(REG_CR + 2)		/*  Media Status register */
diff --git a/drivers/staging/rtl8723bs/include/hal_intf.h b/drivers/staging/rtl8723bs/include/hal_intf.h
index 2fa2382ad5f3..82b60899129d 100644
--- a/drivers/staging/rtl8723bs/include/hal_intf.h
+++ b/drivers/staging/rtl8723bs/include/hal_intf.h
@@ -199,7 +199,6 @@ void rtw_hal_chip_configure(struct adapter *padapter);
 void rtw_hal_read_chip_info(struct adapter *padapter);
 void rtw_hal_read_chip_version(struct adapter *padapter);
 
-u8 rtw_hal_set_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
 u8 rtw_hal_get_def_var(struct adapter *padapter, enum hal_def_variable eVariable, void *pValue);
 
 void rtw_hal_set_odm_var(struct adapter *padapter, enum hal_odm_variable eVariable, void *pValue1, bool bSet);
@@ -262,7 +261,6 @@ void SetHwReg8723BS(struct adapter *padapter, u8 variable, u8 *val);
 void GetHwReg8723BS(struct adapter *padapter, u8 variable, u8 *val);
 void SetHwRegWithBuf8723B(struct adapter *padapter, u8 variable, u8 *pbuf, int len);
 u8 GetHalDefVar8723BSDIO(struct adapter *Adapter, enum hal_def_variable eVariable, void *pValue);
-u8 SetHalDefVar8723BSDIO(struct adapter *Adapter, enum hal_def_variable eVariable, void *pValue);
 void UpdateHalRAMask8723B(struct adapter *padapter, u32 mac_id, u8 rssi_level);
 void rtl8723b_SetBeaconRelatedRegisters(struct adapter *padapter);
 void Hal_EfusePowerSwitch(struct adapter *padapter, u8 PwrState);
diff --git a/drivers/staging/rtl8723bs/include/rtl8723b_hal.h b/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
index 2ed1fc8549ec..06e0a549fa9d 100644
--- a/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
+++ b/drivers/staging/rtl8723bs/include/rtl8723b_hal.h
@@ -223,8 +223,6 @@ void C2HPacketHandler_8723B(struct adapter *padapter, u8 *pbuffer, u16 length);
 
 void SetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val);
 void GetHwReg8723B(struct adapter *padapter, u8 variable, u8 *val);
-u8 SetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable,
-		     void *pval);
 u8 GetHalDefVar8723B(struct adapter *padapter, enum hal_def_variable variable,
 		     void *pval);
 
diff --git a/drivers/staging/rtl8723bs/include/rtw_mlme.h b/drivers/staging/rtl8723bs/include/rtw_mlme.h
index 4c15d0194d4f..2a128568c6df 100644
--- a/drivers/staging/rtl8723bs/include/rtw_mlme.h
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme.h
@@ -18,11 +18,7 @@
 
 #define		SCANNING_TIMEOUT	8000
 
-#ifdef PALTFORM_OS_WINCE
-#define	SCANQUEUE_LIFETIME 12000000 /*  unit:us */
-#else
 #define	SCANQUEUE_LIFETIME 20000 /*  20sec, unit:msec */
-#endif
 
 #define WIFI_NULL_STATE		0x00000000
 #define WIFI_ASOC_STATE		0x00000001		/*  Under Linked state... */
diff --git a/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h b/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
index 53fac838c36a..dd5080056e58 100644
--- a/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723bs/include/rtw_mlme_ext.h
@@ -434,8 +434,8 @@ u8 networktype_to_raid_ex(struct adapter *adapter, struct sta_info *psta);
 
 void get_rate_set(struct adapter *padapter, unsigned char *pbssrate, int *bssrate_len);
 void set_mcs_rate_by_mask(u8 *mcs_set, u32 mask);
-void UpdateBrateTbl(struct adapter *padapter, u8 *mBratesOS);
-void UpdateBrateTblForSoftAP(u8 *bssrateset, u32 bssratelen);
+void update_basic_rate_table(struct adapter *padapter, u8 *mBratesOS);
+void update_basic_rate_table_soft_ap(u8 *bssrateset, u32 bssratelen);
 
 void Save_DM_Func_Flag(struct adapter *padapter);
 void Restore_DM_Func_Flag(struct adapter *padapter);
diff --git a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
index 315bab373729..60edeae1cffe 100644
--- a/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723bs/os_dep/ioctl_cfg80211.c
@@ -1712,7 +1712,8 @@ static int cfg80211_rtw_connect(struct wiphy *wiphy, struct net_device *ndev,
 
 		if (wep_key_len > 0) {
 			wep_key_len = wep_key_len <= 5 ? 5 : 13;
-			wep_total_len = wep_key_len + FIELD_OFFSET(struct ndis_802_11_wep, key_material);
+			wep_total_len = wep_key_len +
+				offsetof(struct ndis_802_11_wep, key_material);
 			pwep = rtw_malloc(wep_total_len);
 			if (!pwep) {
 				ret = -ENOMEM;
diff --git a/drivers/staging/rtl8723bs/os_dep/sdio_intf.c b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
index f3caaa857c86..1d0239eef114 100644
--- a/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
+++ b/drivers/staging/rtl8723bs/os_dep/sdio_intf.c
@@ -490,3 +490,5 @@ static void __exit rtw_drv_halt(void)
 	sdio_unregister_driver(&rtl8723bs_sdio_driver);
 }
 module_exit(rtw_drv_halt);
+
+MODULE_DESCRIPTION("Realtek RTL8723BS SDIO WiFi driver");
diff --git a/drivers/staging/sm750fb/sm750.c b/drivers/staging/sm750fb/sm750.c
index 3659af7e519d..fecd7457e615 100644
--- a/drivers/staging/sm750fb/sm750.c
+++ b/drivers/staging/sm750fb/sm750.c
@@ -121,8 +121,8 @@ static int lynxfb_ops_cursor(struct fb_info *info, struct fb_cursor *fbcursor)
 	sm750_hw_cursor_disable(cursor);
 	if (fbcursor->set & FB_CUR_SETSIZE)
 		sm750_hw_cursor_set_size(cursor,
-					fbcursor->image.width,
-					fbcursor->image.height);
+					 fbcursor->image.width,
+					 fbcursor->image.height);
 
 	if (fbcursor->set & FB_CUR_SETPOS)
 		sm750_hw_cursor_set_pos(cursor,
@@ -537,8 +537,13 @@ static int lynxfb_ops_setcolreg(unsigned int regno,
 		return -EINVAL;
 	}
 
-	if (info->var.grayscale)
-		red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
+	if (info->var.grayscale) {
+		int lum = (red * 77 + green * 151 + blue * 28) >> 8;
+
+		red = lum;
+		green = lum;
+		blue = lum;
+	}
 
 	if (var->bits_per_pixel == 8 &&
 	    info->fix.visual == FB_VISUAL_PSEUDOCOLOR) {
diff --git a/drivers/staging/sm750fb/sm750_accel.c b/drivers/staging/sm750fb/sm750_accel.c
index b07c1aa68621..046b9282b24a 100644
--- a/drivers/staging/sm750fb/sm750_accel.c
+++ b/drivers/staging/sm750fb/sm750_accel.c
@@ -89,7 +89,7 @@ int sm750_hw_fillrect(struct lynx_accel *accel,
 		      u32 x, u32 y, u32 width, u32 height,
 		      u32 color, u32 rop)
 {
-	u32 deCtrl;
+	u32 de_ctrl;
 
 	if (accel->de_wait() != 0) {
 		/*
@@ -121,11 +121,11 @@ int sm750_hw_fillrect(struct lynx_accel *accel,
 		  ((width << DE_DIMENSION_X_SHIFT) & DE_DIMENSION_X_MASK) |
 		  (height & DE_DIMENSION_Y_ET_MASK)); /* dpr8 */
 
-	deCtrl = DE_CONTROL_STATUS | DE_CONTROL_LAST_PIXEL |
+	de_ctrl = DE_CONTROL_STATUS | DE_CONTROL_LAST_PIXEL |
 		DE_CONTROL_COMMAND_RECTANGLE_FILL | DE_CONTROL_ROP_SELECT |
 		(rop & DE_CONTROL_ROP_MASK); /* dpr0xc */
 
-	write_dpr(accel, DE_CONTROL, deCtrl);
+	write_dpr(accel, DE_CONTROL, de_ctrl);
 	return 0;
 }
 
@@ -284,7 +284,7 @@ int sm750_hw_copyarea(struct lynx_accel *accel,
 	return 0;
 }
 
-static unsigned int deGetTransparency(struct lynx_accel *accel)
+static unsigned int de_get_transparency(struct lynx_accel *accel)
 {
 	unsigned int de_ctrl;
 
@@ -391,7 +391,7 @@ int sm750_hw_imageblit(struct lynx_accel *accel, const char *pSrcbuf,
 		DE_CONTROL_ROP_SELECT | DE_CONTROL_COMMAND_HOST_WRITE |
 		DE_CONTROL_HOST | DE_CONTROL_STATUS;
 
-	write_dpr(accel, DE_CONTROL, de_ctrl | deGetTransparency(accel));
+	write_dpr(accel, DE_CONTROL, de_ctrl | de_get_transparency(accel));
 
 	/* Write MONO data (line by line) to 2D Engine data port */
 	for (i = 0; i < height; i++) {
diff --git a/drivers/staging/vc04_services/Kconfig b/drivers/staging/vc04_services/Kconfig
index ccc8e1588648..2f6d1aaffdb2 100644
--- a/drivers/staging/vc04_services/Kconfig
+++ b/drivers/staging/vc04_services/Kconfig
@@ -1,56 +1,7 @@
 # SPDX-License-Identifier: GPL-2.0
-menuconfig BCM_VIDEOCORE
-	tristate "Broadcom VideoCore support"
-	depends on OF
-	depends on RASPBERRYPI_FIRMWARE || (COMPILE_TEST && !RASPBERRYPI_FIRMWARE)
-	default y
-	help
-		Support for Broadcom VideoCore services including
-		the BCM2835 family of products which is used
-		by the Raspberry PI.
-
 if BCM_VIDEOCORE
 
-config BCM2835_VCHIQ
-	tristate "BCM2835 VCHIQ"
-	depends on HAS_DMA
-	imply VCHIQ_CDEV
-	help
-		Broadcom BCM2835 and similar SoCs have a VPU called VideoCore.
-		This config enables the VCHIQ driver, which implements a
-		messaging interface between the kernel and the firmware running
-		on VideoCore. Other drivers use this interface to communicate to
-		the VPU. More specifically, the VCHIQ driver is used by
-		audio/video and camera drivers as well as for implementing MMAL
-		API, which is in turn used by several multimedia services on the
-		BCM2835 family of SoCs.
-
-		Defaults to Y when the Broadcom Videocore services are included
-		in the build, N otherwise.
-
-if BCM2835_VCHIQ
-
-config VCHIQ_CDEV
-	bool "VCHIQ Character Driver"
-	help
-		Enable the creation of VCHIQ character driver. The cdev exposes
-		ioctls used by userspace libraries and testing tools to interact
-		with VideoCore, via the VCHIQ core driver (Check BCM2835_VCHIQ
-		for more info).
-
-		This can be set to 'N' if the VideoCore communication is not
-		needed by userspace but only by other kernel modules
-		(like bcm2835-audio).
-
-		If not sure, set this to 'Y'.
-
-endif
-
 source "drivers/staging/vc04_services/bcm2835-audio/Kconfig"
 
-source "drivers/staging/vc04_services/bcm2835-camera/Kconfig"
-
-source "drivers/staging/vc04_services/vchiq-mmal/Kconfig"
-
 endif
 
diff --git a/drivers/staging/vc04_services/Makefile b/drivers/staging/vc04_services/Makefile
index dad3789522b8..ba15ec663af0 100644
--- a/drivers/staging/vc04_services/Makefile
+++ b/drivers/staging/vc04_services/Makefile
@@ -1,17 +1,3 @@
 # SPDX-License-Identifier: GPL-2.0
-obj-$(CONFIG_BCM2835_VCHIQ)	+= vchiq.o
-
-vchiq-objs := \
-   interface/vchiq_arm/vchiq_core.o  \
-   interface/vchiq_arm/vchiq_arm.o \
-   interface/vchiq_arm/vchiq_bus.o \
-   interface/vchiq_arm/vchiq_debugfs.o \
-
-ifdef CONFIG_VCHIQ_CDEV
-vchiq-objs += interface/vchiq_arm/vchiq_dev.o
-endif
-
 obj-$(CONFIG_SND_BCM2835)		+= bcm2835-audio/
-obj-$(CONFIG_VIDEO_BCM2835)		+= bcm2835-camera/
-obj-$(CONFIG_BCM2835_VCHIQ_MMAL)	+= vchiq-mmal/
 
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
index 0dbe76ee5570..7368b384497f 100644
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-vchiq.c
@@ -4,11 +4,12 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/completion.h>
+
+#include <linux/raspberrypi/vchiq_arm.h>
+
 #include "bcm2835.h"
 #include "vc_vchi_audioserv_defs.h"
 
-#include "../interface/vchiq_arm/vchiq_arm.h"
-
 struct bcm2835_audio_instance {
 	struct device *dev;
 	unsigned int service_handle;
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
index b74cb104e9de..f292a6618166 100644
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
@@ -6,7 +6,8 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 
-#include "../interface/vchiq_arm/vchiq_bus.h"
+#include <linux/raspberrypi/vchiq_bus.h>
+
 #include "bcm2835.h"
 
 static bool enable_hdmi;
diff --git a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
index 49ec5b496edb..5a1348747ff4 100644
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
@@ -5,13 +5,12 @@
 #define __SOUND_ARM_BCM2835_H
 
 #include <linux/device.h>
+#include <linux/raspberrypi/vchiq.h>
 #include <linux/wait.h>
 #include <sound/core.h>
 #include <sound/pcm.h>
 #include <sound/pcm-indirect.h>
 
-#include "../include/linux/raspberrypi/vchiq.h"
-
 #define MAX_SUBSTREAMS   (8)
 #define AVAIL_SUBSTREAMS_MASK  (0xff)
 
diff --git a/drivers/staging/vc04_services/bcm2835-camera/Kconfig b/drivers/staging/vc04_services/bcm2835-camera/Kconfig
deleted file mode 100644
index 870c9afb223a..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/Kconfig
+++ /dev/null
@@ -1,13 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0
-config VIDEO_BCM2835
-	tristate "BCM2835 Camera"
-	depends on MEDIA_SUPPORT
-	depends on VIDEO_DEV && (ARCH_BCM2835 || COMPILE_TEST)
-	select BCM2835_VCHIQ if HAS_DMA
-	select BCM2835_VCHIQ_MMAL if HAS_DMA
-	select VIDEOBUF2_VMALLOC
-	select BTREE
-	help
-	  Say Y here to enable camera host interface devices for
-	  Broadcom BCM2835 SoC. This operates over the VCHIQ interface
-	  to a service running on VideoCore.
diff --git a/drivers/staging/vc04_services/bcm2835-camera/Makefile b/drivers/staging/vc04_services/bcm2835-camera/Makefile
deleted file mode 100644
index 203b93899b20..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/Makefile
+++ /dev/null
@@ -1,6 +0,0 @@
-# SPDX-License-Identifier: GPL-2.0
-bcm2835-v4l2-$(CONFIG_VIDEO_BCM2835) := \
-	bcm2835-camera.o \
-	controls.o
-
-obj-$(CONFIG_VIDEO_BCM2835) += bcm2835-v4l2.o
diff --git a/drivers/staging/vc04_services/bcm2835-camera/TODO b/drivers/staging/vc04_services/bcm2835-camera/TODO
deleted file mode 100644
index 6c2b4ffe4996..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/TODO
+++ /dev/null
@@ -1,17 +0,0 @@
-1) Support dma-buf memory management.
-
-In order to zero-copy import camera images into the 3D or display
-pipelines, we need to export our buffers through dma-buf so that the
-vc4 driver can import them.  This may involve bringing in the VCSM
-driver (which allows long-term management of regions of memory in the
-space that the VPU reserved and Linux otherwise doesn't have access
-to), or building some new protocol that allows VCSM-style management
-of Linux's CMA memory.
-
-2) Avoid extra copies for padding of images.
-
-We expose V4L2_PIX_FMT_* formats that have a specified stride/height
-padding in the V4L2 spec, but that padding doesn't match what the
-hardware can do.  If we exposed the native padding requirements
-through the V4L2 "multiplanar" formats, the firmware would have one
-less copy it needed to do.
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
deleted file mode 100644
index fa7ea4ca4c36..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.c
+++ /dev/null
@@ -1,2011 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * Broadcom BCM2835 V4L2 driver
- *
- * Copyright © 2013 Raspberry Pi (Trading) Ltd.
- *
- * Authors: Vincent Sanders @ Collabora
- *          Dave Stevenson @ Broadcom
- *		(now dave.stevenson@raspberrypi.org)
- *          Simon Mellor @ Broadcom
- *          Luke Diamand @ Broadcom
- */
-
-#include <linux/dma-mapping.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <media/videobuf2-vmalloc.h>
-#include <media/videobuf2-dma-contig.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-fh.h>
-#include <media/v4l2-event.h>
-#include <media/v4l2-common.h>
-#include <linux/delay.h>
-
-#include "../interface/vchiq_arm/vchiq_bus.h"
-#include "../vchiq-mmal/mmal-common.h"
-#include "../vchiq-mmal/mmal-encodings.h"
-#include "../vchiq-mmal/mmal-vchiq.h"
-#include "../vchiq-mmal/mmal-msg.h"
-#include "../vchiq-mmal/mmal-parameters.h"
-#include "bcm2835-camera.h"
-
-#define MIN_WIDTH 32
-#define MIN_HEIGHT 32
-#define MIN_BUFFER_SIZE (80 * 1024)
-
-#define MAX_VIDEO_MODE_WIDTH 1280
-#define MAX_VIDEO_MODE_HEIGHT 720
-
-#define MAX_BCM2835_CAMERAS 2
-
-int bcm2835_v4l2_debug;
-module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
-MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
-
-#define UNSET (-1)
-static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
-module_param_array(video_nr, int, NULL, 0644);
-MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
-
-static int max_video_width = MAX_VIDEO_MODE_WIDTH;
-static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
-module_param(max_video_width, int, 0644);
-MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
-module_param(max_video_height, int, 0644);
-MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
-
-/* camera instance counter */
-static atomic_t camera_instance = ATOMIC_INIT(0);
-
-/* global device data array */
-static struct bcm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
-
-#define FPS_MIN 1
-#define FPS_MAX 90
-
-/* timeperframe: min/max and default */
-static const struct v4l2_fract
-	tpf_min     = {.numerator = 1,		.denominator = FPS_MAX},
-	tpf_max     = {.numerator = 1,	        .denominator = FPS_MIN},
-	tpf_default = {.numerator = 1000,	.denominator = 30000};
-
-/* Container for MMAL and VB2 buffers*/
-struct vb2_mmal_buffer {
-	struct vb2_v4l2_buffer	vb;
-	struct mmal_buffer	mmal;
-};
-
-/* video formats */
-static struct mmal_fmt formats[] = {
-	{
-		.fourcc = V4L2_PIX_FMT_YUV420,
-		.mmal = MMAL_ENCODING_I420,
-		.depth = 12,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 1,
-		.remove_padding = true,
-	}, {
-		.fourcc = V4L2_PIX_FMT_YUYV,
-		.mmal = MMAL_ENCODING_YUYV,
-		.depth = 16,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 2,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_RGB24,
-		.mmal = MMAL_ENCODING_RGB24,
-		.depth = 24,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 3,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_JPEG,
-		.flags = V4L2_FMT_FLAG_COMPRESSED,
-		.mmal = MMAL_ENCODING_JPEG,
-		.depth = 8,
-		.mmal_component = COMP_IMAGE_ENCODE,
-		.ybbp = 0,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_H264,
-		.flags = V4L2_FMT_FLAG_COMPRESSED,
-		.mmal = MMAL_ENCODING_H264,
-		.depth = 8,
-		.mmal_component = COMP_VIDEO_ENCODE,
-		.ybbp = 0,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_MJPEG,
-		.flags = V4L2_FMT_FLAG_COMPRESSED,
-		.mmal = MMAL_ENCODING_MJPEG,
-		.depth = 8,
-		.mmal_component = COMP_VIDEO_ENCODE,
-		.ybbp = 0,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_YVYU,
-		.mmal = MMAL_ENCODING_YVYU,
-		.depth = 16,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 2,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_VYUY,
-		.mmal = MMAL_ENCODING_VYUY,
-		.depth = 16,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 2,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_UYVY,
-		.mmal = MMAL_ENCODING_UYVY,
-		.depth = 16,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 2,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_NV12,
-		.mmal = MMAL_ENCODING_NV12,
-		.depth = 12,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 1,
-		.remove_padding = true,
-	}, {
-		.fourcc = V4L2_PIX_FMT_BGR24,
-		.mmal = MMAL_ENCODING_BGR24,
-		.depth = 24,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 3,
-		.remove_padding = false,
-	}, {
-		.fourcc = V4L2_PIX_FMT_YVU420,
-		.mmal = MMAL_ENCODING_YV12,
-		.depth = 12,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 1,
-		.remove_padding = true,
-	}, {
-		.fourcc = V4L2_PIX_FMT_NV21,
-		.mmal = MMAL_ENCODING_NV21,
-		.depth = 12,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 1,
-		.remove_padding = true,
-	}, {
-		.fourcc = V4L2_PIX_FMT_BGR32,
-		.mmal = MMAL_ENCODING_BGRA,
-		.depth = 32,
-		.mmal_component = COMP_CAMERA,
-		.ybbp = 4,
-		.remove_padding = false,
-	},
-};
-
-static struct mmal_fmt *get_format(struct v4l2_format *f)
-{
-	struct mmal_fmt *fmt;
-	unsigned int k;
-
-	for (k = 0; k < ARRAY_SIZE(formats); k++) {
-		fmt = &formats[k];
-		if (fmt->fourcc == f->fmt.pix.pixelformat)
-			return fmt;
-	}
-
-	return NULL;
-}
-
-/* ------------------------------------------------------------------
- *	Videobuf queue operations
- * ------------------------------------------------------------------
- */
-
-static int queue_setup(struct vb2_queue *vq,
-		       unsigned int *nbuffers, unsigned int *nplanes,
-		       unsigned int sizes[], struct device *alloc_ctxs[])
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
-	unsigned long size;
-
-	/* refuse queue setup if port is not configured */
-	if (!dev->capture.port) {
-		v4l2_err(&dev->v4l2_dev,
-			 "%s: capture port not configured\n", __func__);
-		return -EINVAL;
-	}
-
-	/* Handle CREATE_BUFS situation - *nplanes != 0 */
-	if (*nplanes) {
-		if (*nplanes != 1 ||
-		    sizes[0] < dev->capture.port->current_buffer.size) {
-			v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-				 "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n",
-				 __func__, dev, sizes[0],
-				 dev->capture.port->current_buffer.size,
-				 *nplanes);
-			return -EINVAL;
-		} else {
-			return 0;
-		}
-	}
-
-	/* Handle REQBUFS situation */
-	size = dev->capture.port->current_buffer.size;
-	if (size == 0) {
-		v4l2_err(&dev->v4l2_dev,
-			 "%s: capture port buffer size is zero\n", __func__);
-		return -EINVAL;
-	}
-
-	if (*nbuffers < dev->capture.port->minimum_buffer.num)
-		*nbuffers = dev->capture.port->minimum_buffer.num;
-
-	dev->capture.port->current_buffer.num = *nbuffers;
-
-	*nplanes = 1;
-
-	sizes[0] = size;
-
-	/*
-	 * videobuf2-vmalloc allocator is context-less so no need to set
-	 * alloc_ctxs array.
-	 */
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
-		 __func__, dev);
-
-	return 0;
-}
-
-static int buffer_init(struct vb2_buffer *vb)
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
-	struct vb2_mmal_buffer *buf =
-				container_of(vb2, struct vb2_mmal_buffer, vb);
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
-		 __func__, dev, vb);
-	buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
-	buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
-
-	return mmal_vchi_buffer_init(dev->instance, &buf->mmal);
-}
-
-static int buffer_prepare(struct vb2_buffer *vb)
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	unsigned long size;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
-		 __func__, dev, vb);
-
-	if (!dev->capture.port || !dev->capture.fmt)
-		return -ENODEV;
-
-	size = dev->capture.stride * dev->capture.height;
-	if (vb2_plane_size(vb, 0) < size) {
-		v4l2_err(&dev->v4l2_dev,
-			 "%s data will not fit into plane (%lu < %lu)\n",
-			 __func__, vb2_plane_size(vb, 0), size);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static void buffer_cleanup(struct vb2_buffer *vb)
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
-	struct vb2_mmal_buffer *buf =
-				container_of(vb2, struct vb2_mmal_buffer, vb);
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
-		 __func__, dev, vb);
-
-	mmal_vchi_buffer_cleanup(&buf->mmal);
-}
-
-static inline bool is_capturing(struct bcm2835_mmal_dev *dev)
-{
-	return dev->capture.camera_port ==
-	    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
-}
-
-static void buffer_cb(struct vchiq_mmal_instance *instance,
-		      struct vchiq_mmal_port *port,
-		      int status,
-		      struct mmal_buffer *mmal_buf)
-{
-	struct bcm2835_mmal_dev *dev = port->cb_ctx;
-	struct vb2_mmal_buffer *buf =
-			container_of(mmal_buf, struct vb2_mmal_buffer, mmal);
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
-		 __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags,
-		 mmal_buf->pts);
-
-	if (status) {
-		/* error in transfer */
-		if (buf) {
-			/* there was a buffer with the error so return it */
-			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-		}
-		return;
-	}
-
-	if (mmal_buf->length == 0) {
-		/* stream ended */
-		if (dev->capture.frame_count) {
-			/* empty buffer whilst capturing - expected to be an
-			 * EOS, so grab another frame
-			 */
-			if (is_capturing(dev)) {
-				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-					 "Grab another frame");
-				vchiq_mmal_port_parameter_set(instance,
-							      dev->capture.camera_port,
-							      MMAL_PARAMETER_CAPTURE,
-							      &dev->capture.frame_count,
-							      sizeof(dev->capture.frame_count));
-			}
-			if (vchiq_mmal_submit_buffer(instance, port,
-						     &buf->mmal))
-				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-					 "Failed to return EOS buffer");
-		} else {
-			/* stopping streaming.
-			 * return buffer, and signal frame completion
-			 */
-			vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-			complete(&dev->capture.frame_cmplt);
-		}
-		return;
-	}
-
-	if (!dev->capture.frame_count) {
-		/* signal frame completion */
-		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
-		complete(&dev->capture.frame_cmplt);
-		return;
-	}
-
-	if (dev->capture.vc_start_timestamp != -1 && mmal_buf->pts) {
-		ktime_t timestamp;
-		s64 runtime_us = mmal_buf->pts -
-		    dev->capture.vc_start_timestamp;
-		timestamp = ktime_add_us(dev->capture.kernel_start_ts,
-					 runtime_us);
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Convert start time %llu and %llu with offset %llu to %llu\n",
-			 ktime_to_ns(dev->capture.kernel_start_ts),
-			 dev->capture.vc_start_timestamp, mmal_buf->pts,
-			 ktime_to_ns(timestamp));
-		buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
-	} else {
-		buf->vb.vb2_buf.timestamp = ktime_get_ns();
-	}
-	buf->vb.sequence = dev->capture.sequence++;
-	buf->vb.field = V4L2_FIELD_NONE;
-
-	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length);
-	if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
-		buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME;
-
-	vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
-
-	if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
-	    is_capturing(dev)) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Grab another frame as buffer has EOS");
-		vchiq_mmal_port_parameter_set(instance,
-					      dev->capture.camera_port,
-					      MMAL_PARAMETER_CAPTURE,
-					      &dev->capture.frame_count,
-					      sizeof(dev->capture.frame_count));
-	}
-}
-
-static int enable_camera(struct bcm2835_mmal_dev *dev)
-{
-	int ret;
-
-	if (!dev->camera_use_count) {
-		ret = vchiq_mmal_port_parameter_set(dev->instance,
-						    &dev->component[COMP_CAMERA]->control,
-						    MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
-						    sizeof(dev->camera_num));
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev,
-				 "Failed setting camera num, ret %d\n", ret);
-			return -EINVAL;
-		}
-
-		ret = vchiq_mmal_component_enable(dev->instance,
-						  dev->component[COMP_CAMERA]);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev,
-				 "Failed enabling camera, ret %d\n", ret);
-			return -EINVAL;
-		}
-	}
-	dev->camera_use_count++;
-	v4l2_dbg(1, bcm2835_v4l2_debug,
-		 &dev->v4l2_dev, "enabled camera (refcount %d)\n",
-			dev->camera_use_count);
-	return 0;
-}
-
-static int disable_camera(struct bcm2835_mmal_dev *dev)
-{
-	int ret;
-
-	if (!dev->camera_use_count) {
-		v4l2_err(&dev->v4l2_dev,
-			 "Disabled the camera when already disabled\n");
-		return -EINVAL;
-	}
-	dev->camera_use_count--;
-	if (!dev->camera_use_count) {
-		unsigned int i = 0xFFFFFFFF;
-
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Disabling camera\n");
-		ret = vchiq_mmal_component_disable(dev->instance,
-						   dev->component[COMP_CAMERA]);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev,
-				 "Failed disabling camera, ret %d\n", ret);
-			return -EINVAL;
-		}
-		vchiq_mmal_port_parameter_set(dev->instance,
-					      &dev->component[COMP_CAMERA]->control,
-					      MMAL_PARAMETER_CAMERA_NUM,
-					      &i,
-					      sizeof(i));
-	}
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Camera refcount now %d\n", dev->camera_use_count);
-	return 0;
-}
-
-static void buffer_queue(struct vb2_buffer *vb)
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
-	struct vb2_mmal_buffer *buf =
-				container_of(vb2, struct vb2_mmal_buffer, vb);
-	int ret;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "%s: dev:%p buf:%p, idx %u\n",
-		 __func__, dev, buf, vb2->vb2_buf.index);
-
-	ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port,
-				       &buf->mmal);
-	if (ret < 0)
-		v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
-			 __func__);
-}
-
-static int start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
-	int ret;
-	u32 parameter_size;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
-		 __func__, dev);
-
-	/* ensure a format has actually been set */
-	if (!dev->capture.port)
-		return -EINVAL;
-
-	if (enable_camera(dev) < 0) {
-		v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
-		return -EINVAL;
-	}
-
-	/*init_completion(&dev->capture.frame_cmplt); */
-
-	/* enable frame capture */
-	dev->capture.frame_count = 1;
-
-	/* reset sequence number */
-	dev->capture.sequence = 0;
-
-	/* if the preview is not already running, wait for a few frames for AGC
-	 * to settle down.
-	 */
-	if (!dev->component[COMP_PREVIEW]->enabled)
-		msleep(300);
-
-	/* enable the connection from camera to encoder (if applicable) */
-	if (dev->capture.camera_port != dev->capture.port &&
-	    dev->capture.camera_port) {
-		ret = vchiq_mmal_port_enable(dev->instance,
-					     dev->capture.camera_port, NULL);
-		if (ret) {
-			v4l2_err(&dev->v4l2_dev,
-				 "Failed to enable encode tunnel - error %d\n",
-				 ret);
-			return -1;
-		}
-	}
-
-	/* Get VC timestamp at this point in time */
-	parameter_size = sizeof(dev->capture.vc_start_timestamp);
-	if (vchiq_mmal_port_parameter_get(dev->instance,
-					  dev->capture.camera_port,
-					  MMAL_PARAMETER_SYSTEM_TIME,
-					  &dev->capture.vc_start_timestamp,
-					  &parameter_size)) {
-		v4l2_err(&dev->v4l2_dev,
-			 "Failed to get VC start time - update your VC f/w\n");
-
-		/* Flag to indicate just to rely on kernel timestamps */
-		dev->capture.vc_start_timestamp = -1;
-	} else {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Start time %lld size %d\n",
-			 dev->capture.vc_start_timestamp, parameter_size);
-	}
-
-	dev->capture.kernel_start_ts = ktime_get();
-
-	/* enable the camera port */
-	dev->capture.port->cb_ctx = dev;
-	ret = vchiq_mmal_port_enable(dev->instance, dev->capture.port,
-				     buffer_cb);
-	if (ret) {
-		v4l2_err(&dev->v4l2_dev,
-			 "Failed to enable capture port - error %d. Disabling camera port again\n",
-			 ret);
-
-		vchiq_mmal_port_disable(dev->instance,
-					dev->capture.camera_port);
-		if (disable_camera(dev) < 0) {
-			v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
-			return -EINVAL;
-		}
-		return -1;
-	}
-
-	/* capture the first frame */
-	vchiq_mmal_port_parameter_set(dev->instance,
-				      dev->capture.camera_port,
-				      MMAL_PARAMETER_CAPTURE,
-				      &dev->capture.frame_count,
-				      sizeof(dev->capture.frame_count));
-	return 0;
-}
-
-/* abort streaming and wait for last buffer */
-static void stop_streaming(struct vb2_queue *vq)
-{
-	int ret;
-	unsigned long time_left;
-	struct bcm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
-	struct vchiq_mmal_port *port = dev->capture.port;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
-		 __func__, dev);
-
-	init_completion(&dev->capture.frame_cmplt);
-	dev->capture.frame_count = 0;
-
-	/* ensure a format has actually been set */
-	if (!port) {
-		v4l2_err(&dev->v4l2_dev,
-			 "no capture port - stream not started?\n");
-		return;
-	}
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
-
-	/* stop capturing frames */
-	vchiq_mmal_port_parameter_set(dev->instance,
-				      dev->capture.camera_port,
-				      MMAL_PARAMETER_CAPTURE,
-				      &dev->capture.frame_count,
-				      sizeof(dev->capture.frame_count));
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "disabling connection\n");
-
-	/* disable the connection from camera to encoder */
-	ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
-	if (!ret && dev->capture.camera_port != port) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "disabling port\n");
-		ret = vchiq_mmal_port_disable(dev->instance, port);
-	} else if (dev->capture.camera_port != port) {
-		v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
-			 ret);
-	}
-
-	/* wait for all buffers to be returned */
-	while (atomic_read(&port->buffers_with_vpu)) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s: Waiting for buffers to be returned - %d outstanding\n",
-			 __func__, atomic_read(&port->buffers_with_vpu));
-		time_left = wait_for_completion_timeout(&dev->capture.frame_cmplt,
-							HZ);
-		if (time_left == 0) {
-			v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
-				 __func__,
-				 atomic_read(&port->buffers_with_vpu));
-			break;
-		}
-	}
-
-	if (disable_camera(dev) < 0)
-		v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
-}
-
-static const struct vb2_ops bcm2835_mmal_video_qops = {
-	.queue_setup = queue_setup,
-	.buf_init = buffer_init,
-	.buf_prepare = buffer_prepare,
-	.buf_cleanup = buffer_cleanup,
-	.buf_queue = buffer_queue,
-	.start_streaming = start_streaming,
-	.stop_streaming = stop_streaming,
-};
-
-/* ------------------------------------------------------------------
- *	IOCTL operations
- * ------------------------------------------------------------------
- */
-
-static int set_overlay_params(struct bcm2835_mmal_dev *dev,
-			      struct vchiq_mmal_port *port)
-{
-	struct mmal_parameter_displayregion prev_config = {
-		.set =	MMAL_DISPLAY_SET_LAYER |
-			MMAL_DISPLAY_SET_ALPHA |
-			MMAL_DISPLAY_SET_DEST_RECT |
-			MMAL_DISPLAY_SET_FULLSCREEN,
-		.layer = 2,
-		.alpha = dev->overlay.global_alpha,
-		.fullscreen = 0,
-		.dest_rect = {
-			.x = dev->overlay.w.left,
-			.y = dev->overlay.w.top,
-			.width = dev->overlay.w.width,
-			.height = dev->overlay.w.height,
-		},
-	};
-	return vchiq_mmal_port_parameter_set(dev->instance, port,
-					     MMAL_PARAMETER_DISPLAYREGION,
-					     &prev_config, sizeof(prev_config));
-}
-
-/* overlay ioctl */
-static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
-				       struct v4l2_fmtdesc *f)
-{
-	struct mmal_fmt *fmt;
-
-	if (f->index >= ARRAY_SIZE(formats))
-		return -EINVAL;
-
-	fmt = &formats[f->index];
-
-	f->pixelformat = fmt->fourcc;
-
-	return 0;
-}
-
-static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
-				    struct v4l2_format *f)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-
-	f->fmt.win = dev->overlay;
-
-	return 0;
-}
-
-static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
-				      struct v4l2_format *f)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-
-	f->fmt.win.field = V4L2_FIELD_NONE;
-	f->fmt.win.chromakey = 0;
-	f->fmt.win.clips = NULL;
-	f->fmt.win.clipcount = 0;
-	f->fmt.win.bitmap = NULL;
-
-	v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1,
-			      &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height,
-			      1, 0);
-	v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1,
-			      &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height,
-			      1, 0);
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Overlay: Now w/h %dx%d l/t %dx%d\n",
-		f->fmt.win.w.width, f->fmt.win.w.height,
-		f->fmt.win.w.left, f->fmt.win.w.top);
-
-	v4l2_dump_win_format(1,
-			     bcm2835_v4l2_debug,
-			     &dev->v4l2_dev,
-			     &f->fmt.win,
-			     __func__);
-	return 0;
-}
-
-static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
-				    struct v4l2_format *f)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-
-	vidioc_try_fmt_vid_overlay(file, priv, f);
-
-	dev->overlay = f->fmt.win;
-	if (dev->component[COMP_PREVIEW]->enabled) {
-		set_overlay_params(dev,
-				   &dev->component[COMP_PREVIEW]->input[0]);
-	}
-
-	return 0;
-}
-
-static int vidioc_overlay(struct file *file, void *f, unsigned int on)
-{
-	int ret;
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	struct vchiq_mmal_port *src;
-	struct vchiq_mmal_port *dst;
-
-	if ((on && dev->component[COMP_PREVIEW]->enabled) ||
-	    (!on && !dev->component[COMP_PREVIEW]->enabled))
-		return 0;	/* already in requested state */
-
-	src = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
-
-	if (!on) {
-		/* disconnect preview ports and disable component */
-		ret = vchiq_mmal_port_disable(dev->instance, src);
-		if (!ret)
-			ret = vchiq_mmal_port_connect_tunnel(dev->instance, src,
-							     NULL);
-		if (ret >= 0)
-			ret = vchiq_mmal_component_disable(dev->instance,
-							   dev->component[COMP_PREVIEW]);
-
-		disable_camera(dev);
-		return ret;
-	}
-
-	/* set preview port format and connect it to output */
-	dst = &dev->component[COMP_PREVIEW]->input[0];
-
-	ret = vchiq_mmal_port_set_format(dev->instance, src);
-	if (ret < 0)
-		return ret;
-
-	ret = set_overlay_params(dev, dst);
-	if (ret < 0)
-		return ret;
-
-	if (enable_camera(dev) < 0)
-		return -EINVAL;
-
-	ret = vchiq_mmal_component_enable(dev->instance,
-					  dev->component[COMP_PREVIEW]);
-	if (ret < 0)
-		return ret;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n",
-		 src, dst);
-	ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst);
-	if (ret)
-		return ret;
-
-	return vchiq_mmal_port_enable(dev->instance, src, NULL);
-}
-
-static int vidioc_g_fbuf(struct file *file, void *fh,
-			 struct v4l2_framebuffer *a)
-{
-	/* The video overlay must stay within the framebuffer and can't be
-	 * positioned independently.
-	 */
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	struct vchiq_mmal_port *preview_port =
-		&dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
-
-	a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
-			V4L2_FBUF_CAP_GLOBAL_ALPHA;
-	a->flags = V4L2_FBUF_FLAG_OVERLAY;
-	a->fmt.width = preview_port->es.video.width;
-	a->fmt.height = preview_port->es.video.height;
-	a->fmt.pixelformat = V4L2_PIX_FMT_YUV420;
-	a->fmt.bytesperline = preview_port->es.video.width;
-	a->fmt.sizeimage = (preview_port->es.video.width *
-			       preview_port->es.video.height * 3) >> 1;
-	a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M;
-
-	return 0;
-}
-
-/* input ioctls */
-static int vidioc_enum_input(struct file *file, void *priv,
-			     struct v4l2_input *inp)
-{
-	/* only a single camera input */
-	if (inp->index)
-		return -EINVAL;
-
-	inp->type = V4L2_INPUT_TYPE_CAMERA;
-	snprintf((char *)inp->name, sizeof(inp->name), "Camera %u", inp->index);
-	return 0;
-}
-
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	*i = 0;
-	return 0;
-}
-
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	if (i)
-		return -EINVAL;
-
-	return 0;
-}
-
-/* capture ioctls */
-static int vidioc_querycap(struct file *file, void *priv,
-			   struct v4l2_capability *cap)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	u32 major;
-	u32 minor;
-
-	vchiq_mmal_version(dev->instance, &major, &minor);
-
-	strscpy(cap->driver, "bcm2835 mmal", sizeof(cap->driver));
-	snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d", major, minor);
-
-	snprintf((char *)cap->bus_info, sizeof(cap->bus_info), "platform:%s", dev->v4l2_dev.name);
-	return 0;
-}
-
-static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
-				   struct v4l2_fmtdesc *f)
-{
-	struct mmal_fmt *fmt;
-
-	if (f->index >= ARRAY_SIZE(formats))
-		return -EINVAL;
-
-	fmt = &formats[f->index];
-
-	f->pixelformat = fmt->fourcc;
-
-	return 0;
-}
-
-static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-
-	f->fmt.pix.width = dev->capture.width;
-	f->fmt.pix.height = dev->capture.height;
-	f->fmt.pix.field = V4L2_FIELD_NONE;
-	f->fmt.pix.pixelformat = dev->capture.fmt->fourcc;
-	f->fmt.pix.bytesperline = dev->capture.stride;
-	f->fmt.pix.sizeimage = dev->capture.buffersize;
-
-	if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
-	else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
-	else
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	f->fmt.pix.priv = 0;
-
-	v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
-			     __func__);
-	return 0;
-}
-
-static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-				  struct v4l2_format *f)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	struct mmal_fmt *mfmt;
-
-	mfmt = get_format(f);
-	if (!mfmt) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Fourcc format (0x%08x) unknown.\n",
-			 f->fmt.pix.pixelformat);
-		f->fmt.pix.pixelformat = formats[0].fourcc;
-		mfmt = get_format(f);
-	}
-
-	f->fmt.pix.field = V4L2_FIELD_NONE;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Clipping/aligning %dx%d format %08X\n",
-		 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
-
-	v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1,
-			      &f->fmt.pix.height, MIN_HEIGHT, dev->max_height,
-			      1, 0);
-	f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp;
-	if (!mfmt->remove_padding) {
-		if (mfmt->depth == 24) {
-			/*
-			 * 24bpp is a pain as we can't use simple masking.
-			 * Min stride is width aligned to 16, times 24bpp.
-			 */
-			f->fmt.pix.bytesperline =
-				((f->fmt.pix.width + 15) & ~15) * 3;
-		} else {
-			/*
-			 * GPU isn't removing padding, so stride is aligned to
-			 * 32
-			 */
-			int align_mask = ((32 * mfmt->depth) >> 3) - 1;
-
-			f->fmt.pix.bytesperline =
-				(f->fmt.pix.bytesperline + align_mask) &
-							~align_mask;
-		}
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Not removing padding, so bytes/line = %d\n",
-			 f->fmt.pix.bytesperline);
-	}
-
-	/* Image buffer has to be padded to allow for alignment, even though
-	 * we sometimes then remove that padding before delivering the buffer.
-	 */
-	f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) *
-			(((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3;
-
-	if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) &&
-	    f->fmt.pix.sizeimage < MIN_BUFFER_SIZE)
-		f->fmt.pix.sizeimage = MIN_BUFFER_SIZE;
-
-	if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
-	else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
-	else
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	f->fmt.pix.priv = 0;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Now %dx%d format %08X\n",
-		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
-
-	v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
-			     __func__);
-	return 0;
-}
-
-static int mmal_setup_video_component(struct bcm2835_mmal_dev *dev,
-				      struct v4l2_format *f)
-{
-	bool overlay_enabled = !!dev->component[COMP_PREVIEW]->enabled;
-	struct vchiq_mmal_port *preview_port;
-	int ret;
-
-	preview_port = &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
-
-	/* Preview and encode ports need to match on resolution */
-	if (overlay_enabled) {
-		/* Need to disable the overlay before we can update
-		 * the resolution
-		 */
-		ret = vchiq_mmal_port_disable(dev->instance, preview_port);
-		if (!ret) {
-			ret = vchiq_mmal_port_connect_tunnel(dev->instance,
-							     preview_port,
-							     NULL);
-		}
-	}
-	preview_port->es.video.width = f->fmt.pix.width;
-	preview_port->es.video.height = f->fmt.pix.height;
-	preview_port->es.video.crop.x = 0;
-	preview_port->es.video.crop.y = 0;
-	preview_port->es.video.crop.width = f->fmt.pix.width;
-	preview_port->es.video.crop.height = f->fmt.pix.height;
-	preview_port->es.video.frame_rate.numerator =
-				  dev->capture.timeperframe.denominator;
-	preview_port->es.video.frame_rate.denominator =
-				  dev->capture.timeperframe.numerator;
-	ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
-
-	if (overlay_enabled) {
-		ret = vchiq_mmal_port_connect_tunnel(dev->instance,
-						     preview_port,
-						     &dev->component[COMP_PREVIEW]->input[0]);
-		if (ret)
-			return ret;
-
-		ret = vchiq_mmal_port_enable(dev->instance, preview_port, NULL);
-	}
-
-	return ret;
-}
-
-static int mmal_setup_encode_component(struct bcm2835_mmal_dev *dev,
-				       struct v4l2_format *f,
-				       struct vchiq_mmal_port *port,
-				       struct vchiq_mmal_port *camera_port,
-				       struct vchiq_mmal_component *component)
-{
-	struct mmal_fmt *mfmt = get_format(f);
-	int ret;
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "vid_cap - set up encode comp\n");
-
-	/* configure buffering */
-	camera_port->current_buffer.size = camera_port->recommended_buffer.size;
-	camera_port->current_buffer.num = camera_port->recommended_buffer.num;
-
-	ret = vchiq_mmal_port_connect_tunnel(dev->instance, camera_port,
-					     &component->input[0]);
-	if (ret) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s failed to create connection\n", __func__);
-		/* ensure capture is not going to be tried */
-		dev->capture.port = NULL;
-		return ret;
-	}
-
-	port->es.video.width = f->fmt.pix.width;
-	port->es.video.height = f->fmt.pix.height;
-	port->es.video.crop.x = 0;
-	port->es.video.crop.y = 0;
-	port->es.video.crop.width = f->fmt.pix.width;
-	port->es.video.crop.height = f->fmt.pix.height;
-	port->es.video.frame_rate.numerator =
-		  dev->capture.timeperframe.denominator;
-	port->es.video.frame_rate.denominator =
-		  dev->capture.timeperframe.numerator;
-
-	port->format.encoding = mfmt->mmal;
-	port->format.encoding_variant = 0;
-	/* Set any encoding specific parameters */
-	switch (mfmt->mmal_component) {
-	case COMP_VIDEO_ENCODE:
-		port->format.bitrate = dev->capture.encode_bitrate;
-		break;
-	case COMP_IMAGE_ENCODE:
-		/* Could set EXIF parameters here */
-		break;
-	default:
-		break;
-	}
-
-	ret = vchiq_mmal_port_set_format(dev->instance, port);
-	if (ret) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s failed to set format %dx%d fmt %08X\n",
-			 __func__,
-			 f->fmt.pix.width,
-			 f->fmt.pix.height,
-			 f->fmt.pix.pixelformat);
-		return ret;
-	}
-
-	ret = vchiq_mmal_component_enable(dev->instance, component);
-	if (ret) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s Failed to enable encode components\n", __func__);
-		return ret;
-	}
-
-	/* configure buffering */
-	port->current_buffer.num = 1;
-	port->current_buffer.size = f->fmt.pix.sizeimage;
-	if (port->format.encoding == MMAL_ENCODING_JPEG) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "JPG - buf size now %d was %d\n",
-			 f->fmt.pix.sizeimage,
-			 port->current_buffer.size);
-		port->current_buffer.size =
-		    (f->fmt.pix.sizeimage < (100 << 10)) ?
-		    (100 << 10) : f->fmt.pix.sizeimage;
-	}
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "vid_cap - cur_buf.size set to %d\n", f->fmt.pix.sizeimage);
-	port->current_buffer.alignment = 0;
-
-	return 0;
-}
-
-static int mmal_setup_components(struct bcm2835_mmal_dev *dev,
-				 struct v4l2_format *f)
-{
-	int ret;
-	struct vchiq_mmal_port *port = NULL, *camera_port = NULL;
-	struct vchiq_mmal_component *encode_component = NULL;
-	struct mmal_fmt *mfmt = get_format(f);
-	bool remove_padding;
-
-	if (!mfmt)
-		return -EINVAL;
-
-	if (dev->capture.encode_component) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "vid_cap - disconnect previous tunnel\n");
-
-		/* Disconnect any previous connection */
-		vchiq_mmal_port_connect_tunnel(dev->instance,
-					       dev->capture.camera_port, NULL);
-		dev->capture.camera_port = NULL;
-		ret = vchiq_mmal_component_disable(dev->instance,
-						   dev->capture.encode_component);
-		if (ret)
-			v4l2_err(&dev->v4l2_dev,
-				 "Failed to disable encode component %d\n",
-				 ret);
-
-		dev->capture.encode_component = NULL;
-	}
-	/* format dependent port setup */
-	switch (mfmt->mmal_component) {
-	case COMP_CAMERA:
-		/* Make a further decision on port based on resolution */
-		if (f->fmt.pix.width <= max_video_width &&
-		    f->fmt.pix.height <= max_video_height)
-			camera_port =
-			    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
-		else
-			camera_port =
-			    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
-		port = camera_port;
-		break;
-	case COMP_IMAGE_ENCODE:
-		encode_component = dev->component[COMP_IMAGE_ENCODE];
-		port = &dev->component[COMP_IMAGE_ENCODE]->output[0];
-		camera_port =
-		    &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
-		break;
-	case COMP_VIDEO_ENCODE:
-		encode_component = dev->component[COMP_VIDEO_ENCODE];
-		port = &dev->component[COMP_VIDEO_ENCODE]->output[0];
-		camera_port =
-		    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
-		break;
-	default:
-		break;
-	}
-
-	if (!port)
-		return -EINVAL;
-
-	if (encode_component)
-		camera_port->format.encoding = MMAL_ENCODING_OPAQUE;
-	else
-		camera_port->format.encoding = mfmt->mmal;
-
-	if (dev->rgb_bgr_swapped) {
-		if (camera_port->format.encoding == MMAL_ENCODING_RGB24)
-			camera_port->format.encoding = MMAL_ENCODING_BGR24;
-		else if (camera_port->format.encoding == MMAL_ENCODING_BGR24)
-			camera_port->format.encoding = MMAL_ENCODING_RGB24;
-	}
-
-	remove_padding = mfmt->remove_padding;
-	vchiq_mmal_port_parameter_set(dev->instance, camera_port,
-				      MMAL_PARAMETER_NO_IMAGE_PADDING,
-				      &remove_padding, sizeof(remove_padding));
-
-	camera_port->format.encoding_variant = 0;
-	camera_port->es.video.width = f->fmt.pix.width;
-	camera_port->es.video.height = f->fmt.pix.height;
-	camera_port->es.video.crop.x = 0;
-	camera_port->es.video.crop.y = 0;
-	camera_port->es.video.crop.width = f->fmt.pix.width;
-	camera_port->es.video.crop.height = f->fmt.pix.height;
-	camera_port->es.video.frame_rate.numerator = 0;
-	camera_port->es.video.frame_rate.denominator = 1;
-	camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF;
-
-	ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
-
-	if (!ret &&
-	    camera_port ==
-	    &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
-		ret = mmal_setup_video_component(dev, f);
-	}
-
-	if (ret) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s failed to set format %dx%d %08X\n", __func__,
-			 f->fmt.pix.width, f->fmt.pix.height,
-			 f->fmt.pix.pixelformat);
-		/* ensure capture is not going to be tried */
-		dev->capture.port = NULL;
-		return ret;
-	}
-
-	if (encode_component) {
-		ret = mmal_setup_encode_component(dev, f, port,
-						  camera_port,
-						  encode_component);
-
-		if (ret)
-			return ret;
-	} else {
-		/* configure buffering */
-		camera_port->current_buffer.num = 1;
-		camera_port->current_buffer.size = f->fmt.pix.sizeimage;
-		camera_port->current_buffer.alignment = 0;
-	}
-
-	dev->capture.fmt = mfmt;
-	dev->capture.stride = f->fmt.pix.bytesperline;
-	dev->capture.width = camera_port->es.video.crop.width;
-	dev->capture.height = camera_port->es.video.crop.height;
-	dev->capture.buffersize = port->current_buffer.size;
-
-	/* select port for capture */
-	dev->capture.port = port;
-	dev->capture.camera_port = camera_port;
-	dev->capture.encode_component = encode_component;
-	v4l2_dbg(1, bcm2835_v4l2_debug,
-		 &dev->v4l2_dev,
-		"Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
-		port->format.encoding,
-		dev->capture.width, dev->capture.height,
-		dev->capture.stride, dev->capture.buffersize);
-
-	/* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
-	return ret;
-}
-
-static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	int ret;
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	struct mmal_fmt *mfmt;
-
-	/* try the format to set valid parameters */
-	ret = vidioc_try_fmt_vid_cap(file, priv, f);
-	if (ret) {
-		v4l2_err(&dev->v4l2_dev,
-			 "vid_cap - vidioc_try_fmt_vid_cap failed\n");
-		return ret;
-	}
-
-	/* if a capture is running refuse to set format */
-	if (vb2_is_busy(&dev->capture.vb_vidq)) {
-		v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__);
-		return -EBUSY;
-	}
-
-	/* If the format is unsupported v4l2 says we should switch to
-	 * a supported one and not return an error.
-	 */
-	mfmt = get_format(f);
-	if (!mfmt) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Fourcc format (0x%08x) unknown.\n",
-			 f->fmt.pix.pixelformat);
-		f->fmt.pix.pixelformat = formats[0].fourcc;
-		mfmt = get_format(f);
-	}
-
-	ret = mmal_setup_components(dev, f);
-	if (ret) {
-		v4l2_err(&dev->v4l2_dev,
-			 "%s: failed to setup mmal components: %d\n",
-			 __func__, ret);
-		ret = -EINVAL;
-	}
-
-	return ret;
-}
-
-static int vidioc_enum_framesizes(struct file *file, void *fh,
-				  struct v4l2_frmsizeenum *fsize)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	static const struct v4l2_frmsize_stepwise sizes = {
-		MIN_WIDTH, 0, 2,
-		MIN_HEIGHT, 0, 2
-	};
-	int i;
-
-	if (fsize->index)
-		return -EINVAL;
-	for (i = 0; i < ARRAY_SIZE(formats); i++)
-		if (formats[i].fourcc == fsize->pixel_format)
-			break;
-	if (i == ARRAY_SIZE(formats))
-		return -EINVAL;
-	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	fsize->stepwise = sizes;
-	fsize->stepwise.max_width = dev->max_width;
-	fsize->stepwise.max_height = dev->max_height;
-	return 0;
-}
-
-/* timeperframe is arbitrary and continuous */
-static int vidioc_enum_frameintervals(struct file *file, void *priv,
-				      struct v4l2_frmivalenum *fival)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	int i;
-
-	if (fival->index)
-		return -EINVAL;
-
-	for (i = 0; i < ARRAY_SIZE(formats); i++)
-		if (formats[i].fourcc == fival->pixel_format)
-			break;
-	if (i == ARRAY_SIZE(formats))
-		return -EINVAL;
-
-	/* regarding width & height - we support any within range */
-	if (fival->width < MIN_WIDTH || fival->width > dev->max_width ||
-	    fival->height < MIN_HEIGHT || fival->height > dev->max_height)
-		return -EINVAL;
-
-	fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
-
-	/* fill in stepwise (step=1.0 is required by V4L2 spec) */
-	fival->stepwise.min  = tpf_min;
-	fival->stepwise.max  = tpf_max;
-	fival->stepwise.step = (struct v4l2_fract) {1, 1};
-
-	return 0;
-}
-
-static int vidioc_g_parm(struct file *file, void *priv,
-			 struct v4l2_streamparm *parm)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
-	parm->parm.capture.timeperframe = dev->capture.timeperframe;
-	parm->parm.capture.readbuffers  = 1;
-	return 0;
-}
-
-static int vidioc_s_parm(struct file *file, void *priv,
-			 struct v4l2_streamparm *parm)
-{
-	struct bcm2835_mmal_dev *dev = video_drvdata(file);
-	struct v4l2_fract tpf;
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	tpf = parm->parm.capture.timeperframe;
-
-	/* tpf: {*, 0} resets timing; clip to [min, max]*/
-	tpf = tpf.denominator ? tpf : tpf_default;
-	tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf;
-	tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf;
-
-	dev->capture.timeperframe = tpf;
-	parm->parm.capture.timeperframe = tpf;
-	parm->parm.capture.readbuffers  = 1;
-	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
-
-	set_framerate_params(dev);
-
-	return 0;
-}
-
-static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
-	/* overlay */
-	.vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay,
-	.vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay,
-	.vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay,
-	.vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay,
-	.vidioc_overlay = vidioc_overlay,
-	.vidioc_g_fbuf = vidioc_g_fbuf,
-
-	/* inputs */
-	.vidioc_enum_input = vidioc_enum_input,
-	.vidioc_g_input = vidioc_g_input,
-	.vidioc_s_input = vidioc_s_input,
-
-	/* capture */
-	.vidioc_querycap = vidioc_querycap,
-	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
-
-	/* buffer management */
-	.vidioc_reqbufs = vb2_ioctl_reqbufs,
-	.vidioc_create_bufs = vb2_ioctl_create_bufs,
-	.vidioc_prepare_buf = vb2_ioctl_prepare_buf,
-	.vidioc_querybuf = vb2_ioctl_querybuf,
-	.vidioc_qbuf = vb2_ioctl_qbuf,
-	.vidioc_dqbuf = vb2_ioctl_dqbuf,
-	.vidioc_enum_framesizes = vidioc_enum_framesizes,
-	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
-	.vidioc_g_parm        = vidioc_g_parm,
-	.vidioc_s_parm        = vidioc_s_parm,
-	.vidioc_streamon = vb2_ioctl_streamon,
-	.vidioc_streamoff = vb2_ioctl_streamoff,
-
-	.vidioc_log_status = v4l2_ctrl_log_status,
-	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
-	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
-};
-
-/* ------------------------------------------------------------------
- *	Driver init/finalise
- * ------------------------------------------------------------------
- */
-
-static const struct v4l2_file_operations camera0_fops = {
-	.owner = THIS_MODULE,
-	.open = v4l2_fh_open,
-	.release = vb2_fop_release,
-	.read = vb2_fop_read,
-	.poll = vb2_fop_poll,
-	.unlocked_ioctl = video_ioctl2,	/* V4L2 ioctl handler */
-	.mmap = vb2_fop_mmap,
-};
-
-static const struct video_device vdev_template = {
-	.name = "camera0",
-	.fops = &camera0_fops,
-	.ioctl_ops = &camera0_ioctl_ops,
-	.release = video_device_release_empty,
-	.device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
-		       V4L2_CAP_STREAMING | V4L2_CAP_READWRITE,
-};
-
-/* Returns the number of cameras, and also the max resolution supported
- * by those cameras.
- */
-static int get_num_cameras(struct vchiq_mmal_instance *instance,
-			   unsigned int resolutions[][2], int num_resolutions)
-{
-	int ret;
-	struct vchiq_mmal_component  *cam_info_component;
-	struct mmal_parameter_camera_info cam_info = {0};
-	u32 param_size = sizeof(cam_info);
-	int i;
-
-	/* create a camera_info component */
-	ret = vchiq_mmal_component_init(instance, "camera_info",
-					&cam_info_component);
-	if (ret < 0)
-		/* Unusual failure - let's guess one camera. */
-		return 1;
-
-	if (vchiq_mmal_port_parameter_get(instance,
-					  &cam_info_component->control,
-					  MMAL_PARAMETER_CAMERA_INFO,
-					  &cam_info,
-					  &param_size)) {
-		pr_info("Failed to get camera info\n");
-	}
-	for (i = 0;
-	     i < min_t(unsigned int, cam_info.num_cameras, num_resolutions);
-	     i++) {
-		resolutions[i][0] = cam_info.cameras[i].max_width;
-		resolutions[i][1] = cam_info.cameras[i].max_height;
-	}
-
-	vchiq_mmal_component_finalise(instance,
-				      cam_info_component);
-
-	return cam_info.num_cameras;
-}
-
-static int set_camera_parameters(struct vchiq_mmal_instance *instance,
-				 struct vchiq_mmal_component *camera,
-				 struct bcm2835_mmal_dev *dev)
-{
-	struct mmal_parameter_camera_config cam_config = {
-		.max_stills_w = dev->max_width,
-		.max_stills_h = dev->max_height,
-		.stills_yuv422 = 1,
-		.one_shot_stills = 1,
-		.max_preview_video_w = (max_video_width > 1920) ?
-						max_video_width : 1920,
-		.max_preview_video_h = (max_video_height > 1088) ?
-						max_video_height : 1088,
-		.num_preview_video_frames = 3,
-		.stills_capture_circular_buffer_height = 0,
-		.fast_preview_resume = 0,
-		.use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
-	};
-
-	return vchiq_mmal_port_parameter_set(instance, &camera->control,
-					    MMAL_PARAMETER_CAMERA_CONFIG,
-					    &cam_config, sizeof(cam_config));
-}
-
-#define MAX_SUPPORTED_ENCODINGS 20
-
-/* MMAL instance and component init */
-static int mmal_init(struct bcm2835_mmal_dev *dev)
-{
-	int ret;
-	struct mmal_es_format_local *format;
-	u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
-	u32 param_size;
-	struct vchiq_mmal_component  *camera;
-
-	ret = vchiq_mmal_init(dev->v4l2_dev.dev, &dev->instance);
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n",
-			 __func__, ret);
-		return ret;
-	}
-
-	/* get the camera component ready */
-	ret = vchiq_mmal_component_init(dev->instance, "ril.camera",
-					&dev->component[COMP_CAMERA]);
-	if (ret < 0)
-		goto unreg_mmal;
-
-	camera = dev->component[COMP_CAMERA];
-	if (camera->outputs < CAM_PORT_COUNT) {
-		v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n",
-			 __func__, camera->outputs, CAM_PORT_COUNT);
-		ret = -EINVAL;
-		goto unreg_camera;
-	}
-
-	ret = set_camera_parameters(dev->instance,
-				    camera,
-				    dev);
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n",
-			 __func__, ret);
-		goto unreg_camera;
-	}
-
-	/* There was an error in the firmware that meant the camera component
-	 * produced BGR instead of RGB.
-	 * This is now fixed, but in order to support the old firmwares, we
-	 * have to check.
-	 */
-	dev->rgb_bgr_swapped = true;
-	param_size = sizeof(supported_encodings);
-	ret = vchiq_mmal_port_parameter_get(dev->instance,
-					    &camera->output[CAM_PORT_CAPTURE],
-					    MMAL_PARAMETER_SUPPORTED_ENCODINGS,
-					    &supported_encodings,
-					    &param_size);
-	if (ret == 0) {
-		int i;
-
-		for (i = 0; i < param_size / sizeof(u32); i++) {
-			if (supported_encodings[i] == MMAL_ENCODING_BGR24) {
-				/* Found BGR24 first - old firmware. */
-				break;
-			}
-			if (supported_encodings[i] == MMAL_ENCODING_RGB24) {
-				/* Found RGB24 first
-				 * new firmware, so use RGB24.
-				 */
-				dev->rgb_bgr_swapped = false;
-			break;
-			}
-		}
-	}
-	format = &camera->output[CAM_PORT_PREVIEW].format;
-
-	format->encoding = MMAL_ENCODING_OPAQUE;
-	format->encoding_variant = MMAL_ENCODING_I420;
-
-	format->es->video.width = 1024;
-	format->es->video.height = 768;
-	format->es->video.crop.x = 0;
-	format->es->video.crop.y = 0;
-	format->es->video.crop.width = 1024;
-	format->es->video.crop.height = 768;
-	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
-	format->es->video.frame_rate.denominator = 1;
-
-	format = &camera->output[CAM_PORT_VIDEO].format;
-
-	format->encoding = MMAL_ENCODING_OPAQUE;
-	format->encoding_variant = MMAL_ENCODING_I420;
-
-	format->es->video.width = 1024;
-	format->es->video.height = 768;
-	format->es->video.crop.x = 0;
-	format->es->video.crop.y = 0;
-	format->es->video.crop.width = 1024;
-	format->es->video.crop.height = 768;
-	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
-	format->es->video.frame_rate.denominator = 1;
-
-	format = &camera->output[CAM_PORT_CAPTURE].format;
-
-	format->encoding = MMAL_ENCODING_OPAQUE;
-
-	format->es->video.width = 2592;
-	format->es->video.height = 1944;
-	format->es->video.crop.x = 0;
-	format->es->video.crop.y = 0;
-	format->es->video.crop.width = 2592;
-	format->es->video.crop.height = 1944;
-	format->es->video.frame_rate.numerator = 0; /* Rely on fps_range */
-	format->es->video.frame_rate.denominator = 1;
-
-	dev->capture.width = format->es->video.width;
-	dev->capture.height = format->es->video.height;
-	dev->capture.fmt = &formats[0];
-	dev->capture.encode_component = NULL;
-	dev->capture.timeperframe = tpf_default;
-	dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
-	dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
-
-	/* get the preview component ready */
-	ret = vchiq_mmal_component_init(dev->instance, "ril.video_render",
-					&dev->component[COMP_PREVIEW]);
-	if (ret < 0)
-		goto unreg_camera;
-
-	if (dev->component[COMP_PREVIEW]->inputs < 1) {
-		ret = -EINVAL;
-		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
-			 __func__, dev->component[COMP_PREVIEW]->inputs, 1);
-		goto unreg_preview;
-	}
-
-	/* get the image encoder component ready */
-	ret = vchiq_mmal_component_init(dev->instance, "ril.image_encode",
-					&dev->component[COMP_IMAGE_ENCODE]);
-	if (ret < 0)
-		goto unreg_preview;
-
-	if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) {
-		ret = -EINVAL;
-		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
-			 __func__, dev->component[COMP_IMAGE_ENCODE]->inputs,
-			 1);
-		goto unreg_image_encoder;
-	}
-
-	/* get the video encoder component ready */
-	ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode",
-					&dev->component[COMP_VIDEO_ENCODE]);
-	if (ret < 0)
-		goto unreg_image_encoder;
-
-	if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) {
-		ret = -EINVAL;
-		v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
-			 __func__, dev->component[COMP_VIDEO_ENCODE]->inputs,
-			 1);
-		goto unreg_vid_encoder;
-	}
-
-	{
-		struct vchiq_mmal_port *encoder_port =
-			&dev->component[COMP_VIDEO_ENCODE]->output[0];
-		encoder_port->format.encoding = MMAL_ENCODING_H264;
-		ret = vchiq_mmal_port_set_format(dev->instance,
-						 encoder_port);
-	}
-
-	{
-		unsigned int enable = 1;
-
-		vchiq_mmal_port_parameter_set(dev->instance,
-					      &dev->component[COMP_VIDEO_ENCODE]->control,
-					      MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
-					      &enable,
-					      sizeof(enable));
-
-		vchiq_mmal_port_parameter_set(dev->instance,
-					      &dev->component[COMP_VIDEO_ENCODE]->control,
-					      MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
-					      &enable,
-					      sizeof(enable));
-	}
-	ret = bcm2835_mmal_set_all_camera_controls(dev);
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n",
-			 __func__, ret);
-		goto unreg_vid_encoder;
-	}
-
-	return 0;
-
-unreg_vid_encoder:
-	pr_err("Cleanup: Destroy video encoder\n");
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_VIDEO_ENCODE]);
-
-unreg_image_encoder:
-	pr_err("Cleanup: Destroy image encoder\n");
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_IMAGE_ENCODE]);
-
-unreg_preview:
-	pr_err("Cleanup: Destroy video render\n");
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_PREVIEW]);
-
-unreg_camera:
-	pr_err("Cleanup: Destroy camera\n");
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_CAMERA]);
-
-unreg_mmal:
-	vchiq_mmal_finalise(dev->instance);
-	return ret;
-}
-
-static int bcm2835_mmal_init_device(struct bcm2835_mmal_dev *dev, struct video_device *vfd)
-{
-	int ret;
-
-	*vfd = vdev_template;
-
-	vfd->v4l2_dev = &dev->v4l2_dev;
-
-	vfd->lock = &dev->mutex;
-
-	vfd->queue = &dev->capture.vb_vidq;
-
-	/* video device needs to be able to access instance data */
-	video_set_drvdata(vfd, dev);
-
-	ret = video_register_device(vfd, VFL_TYPE_VIDEO,
-				    video_nr[dev->camera_num]);
-	if (ret < 0)
-		return ret;
-
-	v4l2_info(vfd->v4l2_dev,
-		  "V4L2 device registered as %s - stills mode > %dx%d\n",
-		  video_device_node_name(vfd),
-		  max_video_width, max_video_height);
-
-	return 0;
-}
-
-static void bcm2835_cleanup_instance(struct bcm2835_mmal_dev *dev)
-{
-	if (!dev)
-		return;
-
-	v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
-		  video_device_node_name(&dev->vdev));
-
-	video_unregister_device(&dev->vdev);
-
-	if (dev->capture.encode_component) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "mmal_exit - disconnect tunnel\n");
-		vchiq_mmal_port_connect_tunnel(dev->instance,
-					       dev->capture.camera_port, NULL);
-		vchiq_mmal_component_disable(dev->instance,
-					     dev->capture.encode_component);
-	}
-	vchiq_mmal_component_disable(dev->instance,
-				     dev->component[COMP_CAMERA]);
-
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_VIDEO_ENCODE]);
-
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_IMAGE_ENCODE]);
-
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_PREVIEW]);
-
-	vchiq_mmal_component_finalise(dev->instance,
-				      dev->component[COMP_CAMERA]);
-
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-
-	v4l2_device_unregister(&dev->v4l2_dev);
-
-	kfree(dev);
-}
-
-static struct v4l2_format default_v4l2_format = {
-	.fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
-	.fmt.pix.width = 1024,
-	.fmt.pix.bytesperline = 0,
-	.fmt.pix.height = 768,
-	.fmt.pix.sizeimage = 1024 * 768,
-};
-
-static int bcm2835_mmal_probe(struct vchiq_device *device)
-{
-	int ret;
-	struct bcm2835_mmal_dev *dev;
-	struct vb2_queue *q;
-	int camera;
-	unsigned int num_cameras;
-	struct vchiq_mmal_instance *instance;
-	unsigned int resolutions[MAX_BCM2835_CAMERAS][2];
-	int i;
-
-	ret = dma_set_mask_and_coherent(&device->dev, DMA_BIT_MASK(32));
-	if (ret) {
-		dev_err(&device->dev, "dma_set_mask_and_coherent failed: %d\n", ret);
-		return ret;
-	}
-
-	ret = vchiq_mmal_init(&device->dev, &instance);
-	if (ret < 0)
-		return ret;
-
-	num_cameras = get_num_cameras(instance,
-				      resolutions,
-				      MAX_BCM2835_CAMERAS);
-
-	if (num_cameras < 1) {
-		ret = -ENODEV;
-		goto cleanup_mmal;
-	}
-
-	if (num_cameras > MAX_BCM2835_CAMERAS)
-		num_cameras = MAX_BCM2835_CAMERAS;
-
-	for (camera = 0; camera < num_cameras; camera++) {
-		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-		if (!dev) {
-			ret = -ENOMEM;
-			goto cleanup_gdev;
-		}
-
-		/* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
-		mutex_init(&dev->mutex);
-		dev->max_width = resolutions[camera][0];
-		dev->max_height = resolutions[camera][1];
-
-		/* setup device defaults */
-		dev->overlay.w.left = 150;
-		dev->overlay.w.top = 50;
-		dev->overlay.w.width = 1024;
-		dev->overlay.w.height = 768;
-		dev->overlay.clipcount = 0;
-		dev->overlay.field = V4L2_FIELD_NONE;
-		dev->overlay.global_alpha = 255;
-
-		dev->capture.fmt = &formats[3]; /* JPEG */
-
-		/* v4l device registration */
-		dev->camera_num = v4l2_device_set_name(&dev->v4l2_dev, KBUILD_MODNAME,
-						       &camera_instance);
-		ret = v4l2_device_register(NULL, &dev->v4l2_dev);
-		if (ret) {
-			dev_err(&device->dev, "%s: could not register V4L2 device: %d\n",
-				__func__, ret);
-			goto free_dev;
-		}
-		dev->v4l2_dev.dev = &device->dev;
-
-		/* setup v4l controls */
-		ret = bcm2835_mmal_init_controls(dev, &dev->ctrl_handler);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n",
-				 __func__, ret);
-			goto unreg_dev;
-		}
-		dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
-
-		/* mmal init */
-		dev->instance = instance;
-		ret = mmal_init(dev);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n",
-				 __func__, ret);
-			goto unreg_dev;
-		}
-		/* initialize queue */
-		q = &dev->capture.vb_vidq;
-		memset(q, 0, sizeof(*q));
-		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
-		q->drv_priv = dev;
-		q->buf_struct_size = sizeof(struct vb2_mmal_buffer);
-		q->ops = &bcm2835_mmal_video_qops;
-		q->mem_ops = &vb2_vmalloc_memops;
-		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-		q->lock = &dev->mutex;
-		ret = vb2_queue_init(q);
-		if (ret < 0)
-			goto unreg_dev;
-
-		/* initialise video devices */
-		ret = bcm2835_mmal_init_device(dev, &dev->vdev);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n",
-				 __func__, ret);
-			goto unreg_dev;
-		}
-
-		/* Really want to call vidioc_s_fmt_vid_cap with the default
-		 * format, but currently the APIs don't join up.
-		 */
-		ret = mmal_setup_components(dev, &default_v4l2_format);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n",
-				 __func__, ret);
-			goto unreg_dev;
-		}
-
-		v4l2_info(&dev->v4l2_dev, "Broadcom 2835 MMAL video capture loaded.\n");
-
-		gdev[camera] = dev;
-	}
-	return 0;
-
-unreg_dev:
-	v4l2_ctrl_handler_free(&dev->ctrl_handler);
-	v4l2_device_unregister(&dev->v4l2_dev);
-
-free_dev:
-	kfree(dev);
-
-cleanup_gdev:
-	for (i = 0; i < camera; i++) {
-		bcm2835_cleanup_instance(gdev[i]);
-		gdev[i] = NULL;
-	}
-
-cleanup_mmal:
-	vchiq_mmal_finalise(instance);
-
-	return ret;
-}
-
-static void bcm2835_mmal_remove(struct vchiq_device *device)
-{
-	int camera;
-	struct vchiq_mmal_instance *instance = gdev[0]->instance;
-
-	for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
-		bcm2835_cleanup_instance(gdev[camera]);
-		gdev[camera] = NULL;
-	}
-	vchiq_mmal_finalise(instance);
-}
-
-static const struct vchiq_device_id device_id_table[] = {
-	{ .name = "bcm2835-camera" },
-	{}
-};
-MODULE_DEVICE_TABLE(vchiq, device_id_table);
-
-static struct vchiq_driver bcm2835_camera_driver = {
-	.probe		= bcm2835_mmal_probe,
-	.remove		= bcm2835_mmal_remove,
-	.id_table	= device_id_table,
-	.driver		= {
-		.name	= "bcm2835-camera",
-	},
-};
-
-module_vchiq_driver(bcm2835_camera_driver)
-
-MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
-MODULE_AUTHOR("Vincent Sanders");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h b/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
deleted file mode 100644
index 0f0c6f7a3764..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/bcm2835-camera.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/* SPDX-License-Identifier: GPL-2.0 */
-/*
- * Broadcom BCM2835 V4L2 driver
- *
- * Copyright © 2013 Raspberry Pi (Trading) Ltd.
- *
- * Authors: Vincent Sanders @ Collabora
- *          Dave Stevenson @ Broadcom
- *		(now dave.stevenson@raspberrypi.org)
- *          Simon Mellor @ Broadcom
- *          Luke Diamand @ Broadcom
- *
- * core driver device
- */
-
-#define V4L2_CTRL_COUNT 29 /* number of v4l controls */
-
-enum {
-	COMP_CAMERA = 0,
-	COMP_PREVIEW,
-	COMP_IMAGE_ENCODE,
-	COMP_VIDEO_ENCODE,
-	COMP_COUNT
-};
-
-enum {
-	CAM_PORT_PREVIEW = 0,
-	CAM_PORT_VIDEO,
-	CAM_PORT_CAPTURE,
-	CAM_PORT_COUNT
-};
-
-extern int bcm2835_v4l2_debug;
-
-struct bcm2835_mmal_dev {
-	/* v4l2 devices */
-	struct v4l2_device v4l2_dev;
-	struct video_device vdev;
-	struct mutex mutex;
-
-	/* controls */
-	struct v4l2_ctrl_handler ctrl_handler;
-	struct v4l2_ctrl *ctrls[V4L2_CTRL_COUNT];
-	enum v4l2_scene_mode scene_mode;
-	struct mmal_colourfx colourfx;
-	int hflip;
-	int vflip;
-	int red_gain;
-	int blue_gain;
-	enum mmal_parameter_exposuremode exposure_mode_user;
-	enum v4l2_exposure_auto_type exposure_mode_v4l2_user;
-	/* active exposure mode may differ if selected via a scene mode */
-	enum mmal_parameter_exposuremode exposure_mode_active;
-	enum mmal_parameter_exposuremeteringmode metering_mode;
-	unsigned int manual_shutter_speed;
-	bool exp_auto_priority;
-	bool manual_iso_enabled;
-	u32 iso;
-
-	/* allocated mmal instance and components */
-	struct vchiq_mmal_instance *instance;
-	struct vchiq_mmal_component *component[COMP_COUNT];
-	int camera_use_count;
-
-	struct v4l2_window overlay;
-
-	struct {
-		unsigned int width;  /* width */
-		unsigned int height;  /* height */
-		unsigned int stride;  /* stride */
-		unsigned int buffersize; /* buffer size with padding */
-		struct mmal_fmt *fmt;
-		struct v4l2_fract timeperframe;
-
-		/* H264 encode bitrate */
-		int encode_bitrate;
-		/* H264 bitrate mode. CBR/VBR */
-		int encode_bitrate_mode;
-		/* H264 profile */
-		enum v4l2_mpeg_video_h264_profile enc_profile;
-		/* H264 level */
-		enum v4l2_mpeg_video_h264_level enc_level;
-		/* JPEG Q-factor */
-		int q_factor;
-
-		struct vb2_queue vb_vidq;
-
-		/* VC start timestamp for streaming */
-		s64 vc_start_timestamp;
-		/* Kernel start timestamp for streaming */
-		ktime_t kernel_start_ts;
-		/* Sequence number of last buffer */
-		u32 sequence;
-
-		struct vchiq_mmal_port *port; /* port being used for capture */
-		/* camera port being used for capture */
-		struct vchiq_mmal_port *camera_port;
-		/* component being used for encode */
-		struct vchiq_mmal_component *encode_component;
-		/* number of frames remaining which driver should capture */
-		unsigned int frame_count;
-		/* last frame completion */
-		struct completion frame_cmplt;
-
-	} capture;
-
-	unsigned int camera_num;
-	unsigned int max_width;
-	unsigned int max_height;
-	unsigned int rgb_bgr_swapped;
-};
-
-int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl);
-
-int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev);
-int set_framerate_params(struct bcm2835_mmal_dev *dev);
-
-/* Debug helpers */
-
-#define v4l2_dump_pix_format(level, debug, dev, pix_fmt, desc)	\
-{	\
-	v4l2_dbg(level, debug, dev,	\
-"%s: w %u h %u field %u pfmt 0x%x bpl %u sz_img %u colorspace 0x%x priv %u\n", \
-		desc,	\
-		(pix_fmt)->width, (pix_fmt)->height, (pix_fmt)->field,	\
-		(pix_fmt)->pixelformat, (pix_fmt)->bytesperline,	\
-		(pix_fmt)->sizeimage, (pix_fmt)->colorspace, (pix_fmt)->priv); \
-}
-
-#define v4l2_dump_win_format(level, debug, dev, win_fmt, desc)	\
-{	\
-	v4l2_dbg(level, debug, dev,	\
-"%s: w %u h %u l %u t %u  field %u chromakey %06X clip %p " \
-"clipcount %u bitmap %p\n", \
-		desc,	\
-		(win_fmt)->w.width, (win_fmt)->w.height, \
-		(win_fmt)->w.left, (win_fmt)->w.top, \
-		(win_fmt)->field,	\
-		(win_fmt)->chromakey,	\
-		(win_fmt)->clips, (win_fmt)->clipcount,	\
-		(win_fmt)->bitmap); \
-}
diff --git a/drivers/staging/vc04_services/bcm2835-camera/controls.c b/drivers/staging/vc04_services/bcm2835-camera/controls.c
deleted file mode 100644
index e670226f1edf..000000000000
--- a/drivers/staging/vc04_services/bcm2835-camera/controls.c
+++ /dev/null
@@ -1,1399 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * Broadcom BCM2835 V4L2 driver
- *
- * Copyright © 2013 Raspberry Pi (Trading) Ltd.
- *
- * Authors: Vincent Sanders @ Collabora
- *          Dave Stevenson @ Broadcom
- *		(now dave.stevenson@raspberrypi.org)
- *          Simon Mellor @ Broadcom
- *          Luke Diamand @ Broadcom
- */
-
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <media/videobuf2-vmalloc.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-fh.h>
-#include <media/v4l2-event.h>
-#include <media/v4l2-common.h>
-
-#include "../vchiq-mmal/mmal-common.h"
-#include "../vchiq-mmal/mmal-vchiq.h"
-#include "../vchiq-mmal/mmal-parameters.h"
-#include "bcm2835-camera.h"
-
-/* The supported V4L2_CID_AUTO_EXPOSURE_BIAS values are from -4.0 to +4.0.
- * MMAL values are in 1/6th increments so the MMAL range is -24 to +24.
- * V4L2 docs say value "is expressed in terms of EV, drivers should interpret
- * the values as 0.001 EV units, where the value 1000 stands for +1 EV."
- * V4L2 is limited to a max of 32 values in a menu, so count in 1/3rds from
- * -4 to +4
- */
-static const s64 ev_bias_qmenu[] = {
-	-4000, -3667, -3333,
-	-3000, -2667, -2333,
-	-2000, -1667, -1333,
-	-1000,  -667,  -333,
-	    0,   333,   667,
-	 1000,  1333,  1667,
-	 2000,  2333,  2667,
-	 3000,  3333,  3667,
-	 4000
-};
-
-/* Supported ISO values (*1000)
- * ISOO = auto ISO
- */
-static const s64 iso_qmenu[] = {
-	0, 100000, 200000, 400000, 800000,
-};
-
-static const u32 iso_values[] = {
-	0, 100, 200, 400, 800,
-};
-
-enum bcm2835_mmal_ctrl_type {
-	MMAL_CONTROL_TYPE_STD,
-	MMAL_CONTROL_TYPE_STD_MENU,
-	MMAL_CONTROL_TYPE_INT_MENU,
-	MMAL_CONTROL_TYPE_CLUSTER, /* special cluster entry */
-};
-
-struct bcm2835_mmal_v4l2_ctrl {
-	u32 id; /* v4l2 control identifier */
-	enum bcm2835_mmal_ctrl_type type;
-	/* control minimum value or
-	 * mask for MMAL_CONTROL_TYPE_STD_MENU
-	 */
-	s64 min;
-	s64 max; /* maximum value of control */
-	s64 def;  /* default value of control */
-	u64 step; /* step size of the control */
-	const s64 *imenu; /* integer menu array */
-	u32 mmal_id; /* mmal parameter id */
-	int (*setter)(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl *ctrl,
-		      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl);
-};
-
-struct v4l2_to_mmal_effects_setting {
-	u32 v4l2_effect;
-	u32 mmal_effect;
-	s32 col_fx_enable;
-	s32 col_fx_fixed_cbcr;
-	u32 u;
-	u32 v;
-	u32 num_effect_params;
-	u32 effect_params[MMAL_MAX_IMAGEFX_PARAMETERS];
-};
-
-static const struct v4l2_to_mmal_effects_setting
-	v4l2_to_mmal_effects_values[] = {
-	{  V4L2_COLORFX_NONE,         MMAL_PARAM_IMAGEFX_NONE,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_BW,           MMAL_PARAM_IMAGEFX_NONE,
-		1,   0,    128,  128, 0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SEPIA,        MMAL_PARAM_IMAGEFX_NONE,
-		1,   0,    87,   151, 0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_NEGATIVE,     MMAL_PARAM_IMAGEFX_NEGATIVE,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_EMBOSS,       MMAL_PARAM_IMAGEFX_EMBOSS,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SKETCH,       MMAL_PARAM_IMAGEFX_SKETCH,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SKY_BLUE,     MMAL_PARAM_IMAGEFX_PASTEL,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_GRASS_GREEN,  MMAL_PARAM_IMAGEFX_WATERCOLOUR,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SKIN_WHITEN,  MMAL_PARAM_IMAGEFX_WASHEDOUT,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_VIVID,        MMAL_PARAM_IMAGEFX_SATURATION,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_AQUA,         MMAL_PARAM_IMAGEFX_NONE,
-		1,   0,    171,  121, 0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_ART_FREEZE,   MMAL_PARAM_IMAGEFX_HATCH,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SILHOUETTE,   MMAL_PARAM_IMAGEFX_FILM,
-		0,   0,    0,    0,   0, {0, 0, 0, 0, 0} },
-	{  V4L2_COLORFX_SOLARIZATION, MMAL_PARAM_IMAGEFX_SOLARIZE,
-		0,   0,    0,    0,   5, {1, 128, 160, 160, 48} },
-	{  V4L2_COLORFX_ANTIQUE,      MMAL_PARAM_IMAGEFX_COLOURBALANCE,
-		0,   0,    0,    0,   3, {108, 274, 238, 0, 0} },
-	{  V4L2_COLORFX_SET_CBCR,     MMAL_PARAM_IMAGEFX_NONE,
-		1,   1,    0,    0,   0, {0, 0, 0, 0, 0} }
-};
-
-struct v4l2_mmal_scene_config {
-	enum v4l2_scene_mode v4l2_scene;
-	enum mmal_parameter_exposuremode exposure_mode;
-	enum mmal_parameter_exposuremeteringmode metering_mode;
-};
-
-static const struct v4l2_mmal_scene_config scene_configs[] = {
-	/* V4L2_SCENE_MODE_NONE automatically added */
-	{
-		V4L2_SCENE_MODE_NIGHT,
-		MMAL_PARAM_EXPOSUREMODE_NIGHT,
-		MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE
-	},
-	{
-		V4L2_SCENE_MODE_SPORTS,
-		MMAL_PARAM_EXPOSUREMODE_SPORTS,
-		MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE
-	},
-};
-
-/* control handlers*/
-
-static int ctrl_set_rational(struct bcm2835_mmal_dev *dev,
-			     struct v4l2_ctrl *ctrl,
-			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	struct s32_fract rational_value;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	rational_value.numerator = ctrl->val;
-	rational_value.denominator = 100;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &rational_value,
-					     sizeof(rational_value));
-}
-
-static int ctrl_set_value(struct bcm2835_mmal_dev *dev,
-			  struct v4l2_ctrl *ctrl,
-			  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	u32_value = ctrl->val;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_iso(struct bcm2835_mmal_dev *dev,
-			struct v4l2_ctrl *ctrl,
-			const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *control;
-
-	if (ctrl->val > mmal_ctrl->max || ctrl->val < mmal_ctrl->min)
-		return 1;
-
-	if (ctrl->id == V4L2_CID_ISO_SENSITIVITY)
-		dev->iso = iso_values[ctrl->val];
-	else if (ctrl->id == V4L2_CID_ISO_SENSITIVITY_AUTO)
-		dev->manual_iso_enabled =
-				(ctrl->val == V4L2_ISO_SENSITIVITY_MANUAL);
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	if (dev->manual_iso_enabled)
-		u32_value = dev->iso;
-	else
-		u32_value = 0;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     MMAL_PARAMETER_ISO,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_value_ev(struct bcm2835_mmal_dev *dev,
-			     struct v4l2_ctrl *ctrl,
-			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	s32 s32_value;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	s32_value = (ctrl->val - 12) * 2;	/* Convert from index to 1/6ths */
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &s32_value, sizeof(s32_value));
-}
-
-static int ctrl_set_rotate(struct bcm2835_mmal_dev *dev,
-			   struct v4l2_ctrl *ctrl,
-			   const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret;
-	u32 u32_value;
-	struct vchiq_mmal_component *camera;
-
-	camera = dev->component[COMP_CAMERA];
-
-	u32_value = ((ctrl->val % 360) / 90) * 90;
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-	if (ret < 0)
-		return ret;
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-	if (ret < 0)
-		return ret;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_flip(struct bcm2835_mmal_dev *dev,
-			 struct v4l2_ctrl *ctrl,
-			 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret;
-	u32 u32_value;
-	struct vchiq_mmal_component *camera;
-
-	if (ctrl->id == V4L2_CID_HFLIP)
-		dev->hflip = ctrl->val;
-	else
-		dev->vflip = ctrl->val;
-
-	camera = dev->component[COMP_CAMERA];
-
-	if (dev->hflip && dev->vflip)
-		u32_value = MMAL_PARAM_MIRROR_BOTH;
-	else if (dev->hflip)
-		u32_value = MMAL_PARAM_MIRROR_HORIZONTAL;
-	else if (dev->vflip)
-		u32_value = MMAL_PARAM_MIRROR_VERTICAL;
-	else
-		u32_value = MMAL_PARAM_MIRROR_NONE;
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[0],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-	if (ret < 0)
-		return ret;
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, &camera->output[1],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-	if (ret < 0)
-		return ret;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, &camera->output[2],
-					    mmal_ctrl->mmal_id,
-					    &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_exposure(struct bcm2835_mmal_dev *dev,
-			     struct v4l2_ctrl *ctrl,
-			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	enum mmal_parameter_exposuremode exp_mode = dev->exposure_mode_user;
-	u32 shutter_speed = 0;
-	struct vchiq_mmal_port *control;
-	int ret = 0;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	if (mmal_ctrl->mmal_id == MMAL_PARAMETER_SHUTTER_SPEED)	{
-		/* V4L2 is in 100usec increments.
-		 * MMAL is 1usec.
-		 */
-		dev->manual_shutter_speed = ctrl->val * 100;
-	} else if (mmal_ctrl->mmal_id == MMAL_PARAMETER_EXPOSURE_MODE) {
-		switch (ctrl->val) {
-		case V4L2_EXPOSURE_AUTO:
-			exp_mode = MMAL_PARAM_EXPOSUREMODE_AUTO;
-			break;
-
-		case V4L2_EXPOSURE_MANUAL:
-			exp_mode = MMAL_PARAM_EXPOSUREMODE_OFF;
-			break;
-		}
-		dev->exposure_mode_user = exp_mode;
-		dev->exposure_mode_v4l2_user = ctrl->val;
-	} else if (mmal_ctrl->id == V4L2_CID_EXPOSURE_AUTO_PRIORITY) {
-		dev->exp_auto_priority = ctrl->val;
-	}
-
-	if (dev->scene_mode == V4L2_SCENE_MODE_NONE) {
-		if (exp_mode == MMAL_PARAM_EXPOSUREMODE_OFF)
-			shutter_speed = dev->manual_shutter_speed;
-
-		ret = vchiq_mmal_port_parameter_set(dev->instance,
-						    control,
-						    MMAL_PARAMETER_SHUTTER_SPEED,
-						    &shutter_speed,
-						    sizeof(shutter_speed));
-		ret += vchiq_mmal_port_parameter_set(dev->instance,
-						     control,
-						     MMAL_PARAMETER_EXPOSURE_MODE,
-						     &exp_mode,
-						     sizeof(u32));
-		dev->exposure_mode_active = exp_mode;
-	}
-	/* exposure_dynamic_framerate (V4L2_CID_EXPOSURE_AUTO_PRIORITY) should
-	 * always apply irrespective of scene mode.
-	 */
-	ret += set_framerate_params(dev);
-
-	return ret;
-}
-
-static int ctrl_set_metering_mode(struct bcm2835_mmal_dev *dev,
-				  struct v4l2_ctrl *ctrl,
-				  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	switch (ctrl->val) {
-	case V4L2_EXPOSURE_METERING_AVERAGE:
-		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_AVERAGE;
-		break;
-
-	case V4L2_EXPOSURE_METERING_CENTER_WEIGHTED:
-		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_BACKLIT;
-		break;
-
-	case V4L2_EXPOSURE_METERING_SPOT:
-		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_SPOT;
-		break;
-
-	case V4L2_EXPOSURE_METERING_MATRIX:
-		dev->metering_mode = MMAL_PARAM_EXPOSUREMETERINGMODE_MATRIX;
-		break;
-	}
-
-	if (dev->scene_mode == V4L2_SCENE_MODE_NONE) {
-		struct vchiq_mmal_port *control;
-		u32 u32_value = dev->metering_mode;
-
-		control = &dev->component[COMP_CAMERA]->control;
-
-		return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-	} else {
-		return 0;
-	}
-}
-
-static int ctrl_set_flicker_avoidance(struct bcm2835_mmal_dev *dev,
-				      struct v4l2_ctrl *ctrl,
-				      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	switch (ctrl->val) {
-	case V4L2_CID_POWER_LINE_FREQUENCY_DISABLED:
-		u32_value = MMAL_PARAM_FLICKERAVOID_OFF;
-		break;
-	case V4L2_CID_POWER_LINE_FREQUENCY_50HZ:
-		u32_value = MMAL_PARAM_FLICKERAVOID_50HZ;
-		break;
-	case V4L2_CID_POWER_LINE_FREQUENCY_60HZ:
-		u32_value = MMAL_PARAM_FLICKERAVOID_60HZ;
-		break;
-	case V4L2_CID_POWER_LINE_FREQUENCY_AUTO:
-		u32_value = MMAL_PARAM_FLICKERAVOID_AUTO;
-		break;
-	}
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_awb_mode(struct bcm2835_mmal_dev *dev,
-			     struct v4l2_ctrl *ctrl,
-			     const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	switch (ctrl->val) {
-	case V4L2_WHITE_BALANCE_MANUAL:
-		u32_value = MMAL_PARAM_AWBMODE_OFF;
-		break;
-
-	case V4L2_WHITE_BALANCE_AUTO:
-		u32_value = MMAL_PARAM_AWBMODE_AUTO;
-		break;
-
-	case V4L2_WHITE_BALANCE_INCANDESCENT:
-		u32_value = MMAL_PARAM_AWBMODE_INCANDESCENT;
-		break;
-
-	case V4L2_WHITE_BALANCE_FLUORESCENT:
-		u32_value = MMAL_PARAM_AWBMODE_FLUORESCENT;
-		break;
-
-	case V4L2_WHITE_BALANCE_FLUORESCENT_H:
-		u32_value = MMAL_PARAM_AWBMODE_TUNGSTEN;
-		break;
-
-	case V4L2_WHITE_BALANCE_HORIZON:
-		u32_value = MMAL_PARAM_AWBMODE_HORIZON;
-		break;
-
-	case V4L2_WHITE_BALANCE_DAYLIGHT:
-		u32_value = MMAL_PARAM_AWBMODE_SUNLIGHT;
-		break;
-
-	case V4L2_WHITE_BALANCE_FLASH:
-		u32_value = MMAL_PARAM_AWBMODE_FLASH;
-		break;
-
-	case V4L2_WHITE_BALANCE_CLOUDY:
-		u32_value = MMAL_PARAM_AWBMODE_CLOUDY;
-		break;
-
-	case V4L2_WHITE_BALANCE_SHADE:
-		u32_value = MMAL_PARAM_AWBMODE_SHADE;
-		break;
-	}
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_awb_gains(struct bcm2835_mmal_dev *dev,
-			      struct v4l2_ctrl *ctrl,
-			      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	struct vchiq_mmal_port *control;
-	struct mmal_parameter_awbgains gains;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	if (ctrl->id == V4L2_CID_RED_BALANCE)
-		dev->red_gain = ctrl->val;
-	else if (ctrl->id == V4L2_CID_BLUE_BALANCE)
-		dev->blue_gain = ctrl->val;
-
-	gains.r_gain.numerator = dev->red_gain;
-	gains.r_gain.denominator = 1000;
-	gains.b_gain.numerator = dev->blue_gain;
-	gains.b_gain.denominator = 1000;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, control,
-					     mmal_ctrl->mmal_id,
-					     &gains, sizeof(gains));
-}
-
-static int ctrl_set_image_effect(struct bcm2835_mmal_dev *dev,
-				 struct v4l2_ctrl *ctrl,
-				 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret = -EINVAL;
-	int i, j;
-	struct vchiq_mmal_port *control;
-	struct mmal_parameter_imagefx_parameters imagefx;
-
-	for (i = 0; i < ARRAY_SIZE(v4l2_to_mmal_effects_values); i++) {
-		if (ctrl->val != v4l2_to_mmal_effects_values[i].v4l2_effect)
-			continue;
-
-		imagefx.effect =
-			v4l2_to_mmal_effects_values[i].mmal_effect;
-		imagefx.num_effect_params =
-			v4l2_to_mmal_effects_values[i].num_effect_params;
-
-		if (imagefx.num_effect_params > MMAL_MAX_IMAGEFX_PARAMETERS)
-			imagefx.num_effect_params = MMAL_MAX_IMAGEFX_PARAMETERS;
-
-		for (j = 0; j < imagefx.num_effect_params; j++)
-			imagefx.effect_parameter[j] =
-				v4l2_to_mmal_effects_values[i].effect_params[j];
-
-		dev->colourfx.enable =
-			v4l2_to_mmal_effects_values[i].col_fx_enable;
-		if (!v4l2_to_mmal_effects_values[i].col_fx_fixed_cbcr) {
-			dev->colourfx.u = v4l2_to_mmal_effects_values[i].u;
-			dev->colourfx.v = v4l2_to_mmal_effects_values[i].v;
-		}
-
-		control = &dev->component[COMP_CAMERA]->control;
-
-		ret = vchiq_mmal_port_parameter_set(dev->instance, control,
-						    MMAL_PARAMETER_IMAGE_EFFECT_PARAMETERS,
-						    &imagefx, sizeof(imagefx));
-		if (ret)
-			goto exit;
-
-		ret = vchiq_mmal_port_parameter_set(dev->instance, control,
-						    MMAL_PARAMETER_COLOUR_EFFECT,
-						    &dev->colourfx, sizeof(dev->colourfx));
-	}
-
-exit:
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "mmal_ctrl:%p ctrl id:0x%x ctrl val:%d imagefx:0x%x color_effect:%s u:%d v:%d ret %d(%d)\n",
-				mmal_ctrl, ctrl->id, ctrl->val, imagefx.effect,
-				dev->colourfx.enable ? "true" : "false",
-				dev->colourfx.u, dev->colourfx.v,
-				ret, (ret == 0 ? 0 : -EINVAL));
-	return (ret == 0 ? 0 : -EINVAL);
-}
-
-static int ctrl_set_colfx(struct bcm2835_mmal_dev *dev,
-			  struct v4l2_ctrl *ctrl,
-			  const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret;
-	struct vchiq_mmal_port *control;
-
-	control = &dev->component[COMP_CAMERA]->control;
-
-	dev->colourfx.u = (ctrl->val & 0xff00) >> 8;
-	dev->colourfx.v = ctrl->val & 0xff;
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, control,
-					    MMAL_PARAMETER_COLOUR_EFFECT,
-					    &dev->colourfx,
-					    sizeof(dev->colourfx));
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
-			__func__, mmal_ctrl, ctrl->id, ctrl->val, ret,
-			(ret == 0 ? 0 : -EINVAL));
-	return (ret == 0 ? 0 : -EINVAL);
-}
-
-static int ctrl_set_bitrate(struct bcm2835_mmal_dev *dev,
-			    struct v4l2_ctrl *ctrl,
-			    const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret;
-	struct vchiq_mmal_port *encoder_out;
-
-	dev->capture.encode_bitrate = ctrl->val;
-
-	encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0];
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance, encoder_out,
-					    mmal_ctrl->mmal_id, &ctrl->val,
-					    sizeof(ctrl->val));
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "%s: After: mmal_ctrl:%p ctrl id:0x%x ctrl val:%d ret %d(%d)\n",
-		 __func__, mmal_ctrl, ctrl->id, ctrl->val, ret,
-		 (ret == 0 ? 0 : -EINVAL));
-
-	/*
-	 * Older firmware versions (pre July 2019) have a bug in handling
-	 * MMAL_PARAMETER_VIDEO_BIT_RATE that result in the call
-	 * returning -MMAL_MSG_STATUS_EINVAL. So ignore errors from this call.
-	 */
-	return 0;
-}
-
-static int ctrl_set_bitrate_mode(struct bcm2835_mmal_dev *dev,
-				 struct v4l2_ctrl *ctrl,
-				 const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 bitrate_mode;
-	struct vchiq_mmal_port *encoder_out;
-
-	encoder_out = &dev->component[COMP_VIDEO_ENCODE]->output[0];
-
-	dev->capture.encode_bitrate_mode = ctrl->val;
-	switch (ctrl->val) {
-	default:
-	case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
-		bitrate_mode = MMAL_VIDEO_RATECONTROL_VARIABLE;
-		break;
-	case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
-		bitrate_mode = MMAL_VIDEO_RATECONTROL_CONSTANT;
-		break;
-	}
-
-	vchiq_mmal_port_parameter_set(dev->instance, encoder_out,
-				      mmal_ctrl->mmal_id,
-					     &bitrate_mode,
-					     sizeof(bitrate_mode));
-	return 0;
-}
-
-static int ctrl_set_image_encode_output(struct bcm2835_mmal_dev *dev,
-					struct v4l2_ctrl *ctrl,
-					const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *jpeg_out;
-
-	jpeg_out = &dev->component[COMP_IMAGE_ENCODE]->output[0];
-
-	u32_value = ctrl->val;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, jpeg_out,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_video_encode_param_output(struct bcm2835_mmal_dev *dev,
-					      struct v4l2_ctrl *ctrl,
-					      const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	u32 u32_value;
-	struct vchiq_mmal_port *vid_enc_ctl;
-
-	vid_enc_ctl = &dev->component[COMP_VIDEO_ENCODE]->output[0];
-
-	u32_value = ctrl->val;
-
-	return vchiq_mmal_port_parameter_set(dev->instance, vid_enc_ctl,
-					     mmal_ctrl->mmal_id,
-					     &u32_value, sizeof(u32_value));
-}
-
-static int ctrl_set_video_encode_profile_level(struct bcm2835_mmal_dev *dev,
-					       struct v4l2_ctrl *ctrl,
-					       const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	struct mmal_parameter_video_profile param;
-	int ret = 0;
-
-	if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_PROFILE) {
-		switch (ctrl->val) {
-		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
-		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
-		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
-			dev->capture.enc_profile = ctrl->val;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-	} else if (ctrl->id == V4L2_CID_MPEG_VIDEO_H264_LEVEL) {
-		switch (ctrl->val) {
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
-		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
-			dev->capture.enc_level = ctrl->val;
-			break;
-		default:
-			ret = -EINVAL;
-			break;
-		}
-	}
-
-	if (!ret) {
-		switch (dev->capture.enc_profile) {
-		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
-			param.profile = MMAL_VIDEO_PROFILE_H264_BASELINE;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE:
-			param.profile =
-				MMAL_VIDEO_PROFILE_H264_CONSTRAINED_BASELINE;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
-			param.profile = MMAL_VIDEO_PROFILE_H264_MAIN;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
-			param.profile = MMAL_VIDEO_PROFILE_H264_HIGH;
-			break;
-		default:
-			/* Should never get here */
-			break;
-		}
-
-		switch (dev->capture.enc_level) {
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
-			param.level = MMAL_VIDEO_LEVEL_H264_1;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
-			param.level = MMAL_VIDEO_LEVEL_H264_1b;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
-			param.level = MMAL_VIDEO_LEVEL_H264_11;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
-			param.level = MMAL_VIDEO_LEVEL_H264_12;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
-			param.level = MMAL_VIDEO_LEVEL_H264_13;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
-			param.level = MMAL_VIDEO_LEVEL_H264_2;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
-			param.level = MMAL_VIDEO_LEVEL_H264_21;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
-			param.level = MMAL_VIDEO_LEVEL_H264_22;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
-			param.level = MMAL_VIDEO_LEVEL_H264_3;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
-			param.level = MMAL_VIDEO_LEVEL_H264_31;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
-			param.level = MMAL_VIDEO_LEVEL_H264_32;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
-			param.level = MMAL_VIDEO_LEVEL_H264_4;
-			break;
-		default:
-			/* Should never get here */
-			break;
-		}
-
-		ret = vchiq_mmal_port_parameter_set(dev->instance,
-						    &dev->component[COMP_VIDEO_ENCODE]->output[0],
-			mmal_ctrl->mmal_id,
-			&param, sizeof(param));
-	}
-	return ret;
-}
-
-static int ctrl_set_scene_mode(struct bcm2835_mmal_dev *dev,
-			       struct v4l2_ctrl *ctrl,
-			       const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl)
-{
-	int ret = 0;
-	int shutter_speed;
-	struct vchiq_mmal_port *control;
-
-	v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "scene mode selected %d, was %d\n", ctrl->val,
-		 dev->scene_mode);
-	control = &dev->component[COMP_CAMERA]->control;
-
-	if (ctrl->val == dev->scene_mode)
-		return 0;
-
-	if (ctrl->val == V4L2_SCENE_MODE_NONE) {
-		/* Restore all user selections */
-		dev->scene_mode = V4L2_SCENE_MODE_NONE;
-
-		if (dev->exposure_mode_user == MMAL_PARAM_EXPOSUREMODE_OFF)
-			shutter_speed = dev->manual_shutter_speed;
-		else
-			shutter_speed = 0;
-
-		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n",
-			 __func__, shutter_speed, dev->exposure_mode_user,
-			 dev->metering_mode);
-		ret = vchiq_mmal_port_parameter_set(dev->instance,
-						    control,
-						    MMAL_PARAMETER_SHUTTER_SPEED,
-						    &shutter_speed,
-						    sizeof(shutter_speed));
-		ret += vchiq_mmal_port_parameter_set(dev->instance,
-						     control,
-						     MMAL_PARAMETER_EXPOSURE_MODE,
-						     &dev->exposure_mode_user,
-						     sizeof(u32));
-		dev->exposure_mode_active = dev->exposure_mode_user;
-		ret += vchiq_mmal_port_parameter_set(dev->instance,
-						     control,
-						     MMAL_PARAMETER_EXP_METERING_MODE,
-						     &dev->metering_mode,
-						     sizeof(u32));
-		ret += set_framerate_params(dev);
-	} else {
-		/* Set up scene mode */
-		int i;
-		const struct v4l2_mmal_scene_config *scene = NULL;
-		int shutter_speed;
-		enum mmal_parameter_exposuremode exposure_mode;
-		enum mmal_parameter_exposuremeteringmode metering_mode;
-
-		for (i = 0; i < ARRAY_SIZE(scene_configs); i++) {
-			if (scene_configs[i].v4l2_scene == ctrl->val) {
-				scene = &scene_configs[i];
-				break;
-			}
-		}
-		if (!scene)
-			return -EINVAL;
-		if (i >= ARRAY_SIZE(scene_configs))
-			return -EINVAL;
-
-		/* Set all the values */
-		dev->scene_mode = ctrl->val;
-
-		if (scene->exposure_mode == MMAL_PARAM_EXPOSUREMODE_OFF)
-			shutter_speed = dev->manual_shutter_speed;
-		else
-			shutter_speed = 0;
-		exposure_mode = scene->exposure_mode;
-		metering_mode = scene->metering_mode;
-
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s: scene mode none: shut_speed %d, exp_mode %d, metering %d\n",
-			 __func__, shutter_speed, exposure_mode, metering_mode);
-
-		ret = vchiq_mmal_port_parameter_set(dev->instance, control,
-						    MMAL_PARAMETER_SHUTTER_SPEED,
-						    &shutter_speed,
-						    sizeof(shutter_speed));
-		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
-						     MMAL_PARAMETER_EXPOSURE_MODE,
-						     &exposure_mode,
-						     sizeof(u32));
-		dev->exposure_mode_active = exposure_mode;
-		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
-						     MMAL_PARAMETER_EXPOSURE_MODE,
-						     &exposure_mode,
-						     sizeof(u32));
-		ret += vchiq_mmal_port_parameter_set(dev->instance, control,
-						     MMAL_PARAMETER_EXP_METERING_MODE,
-						     &metering_mode,
-						     sizeof(u32));
-		ret += set_framerate_params(dev);
-	}
-	if (ret) {
-		v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "%s: Setting scene to %d, ret=%d\n",
-			 __func__, ctrl->val, ret);
-		ret = -EINVAL;
-	}
-	return 0;
-}
-
-static int bcm2835_mmal_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct bcm2835_mmal_dev *dev = container_of(ctrl->handler, struct bcm2835_mmal_dev,
-						    ctrl_handler);
-	const struct bcm2835_mmal_v4l2_ctrl *mmal_ctrl = ctrl->priv;
-	int ret;
-
-	if (!mmal_ctrl || mmal_ctrl->id != ctrl->id || !mmal_ctrl->setter) {
-		pr_warn("mmal_ctrl:%p ctrl id:%d\n", mmal_ctrl, ctrl->id);
-		return -EINVAL;
-	}
-
-	ret = mmal_ctrl->setter(dev, ctrl, mmal_ctrl);
-	if (ret)
-		pr_warn("ctrl id:%d/MMAL param %08X- returned ret %d\n",
-			ctrl->id, mmal_ctrl->mmal_id, ret);
-	return ret;
-}
-
-static const struct v4l2_ctrl_ops bcm2835_mmal_ctrl_ops = {
-	.s_ctrl = bcm2835_mmal_s_ctrl,
-};
-
-static const struct bcm2835_mmal_v4l2_ctrl v4l2_ctrls[V4L2_CTRL_COUNT] = {
-	{
-		.id = V4L2_CID_SATURATION,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = -100,
-		.max = 100,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_SATURATION,
-		.setter = ctrl_set_rational,
-	},
-	{
-		.id = V4L2_CID_SHARPNESS,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = -100,
-		.max = 100,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_SHARPNESS,
-		.setter = ctrl_set_rational,
-	},
-	{
-		.id = V4L2_CID_CONTRAST,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = -100,
-		.max = 100,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_CONTRAST,
-		.setter = ctrl_set_rational,
-	},
-	{
-		.id = V4L2_CID_BRIGHTNESS,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 100,
-		.def = 50,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_BRIGHTNESS,
-		.setter = ctrl_set_rational,
-	},
-	{
-		.id = V4L2_CID_ISO_SENSITIVITY,
-		.type = MMAL_CONTROL_TYPE_INT_MENU,
-		.min = 0,
-		.max = ARRAY_SIZE(iso_qmenu) - 1,
-		.def = 0,
-		.step = 1,
-		.imenu = iso_qmenu,
-		.mmal_id = MMAL_PARAMETER_ISO,
-		.setter = ctrl_set_iso,
-	},
-	{
-		.id = V4L2_CID_ISO_SENSITIVITY_AUTO,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = 0,
-		.max = V4L2_ISO_SENSITIVITY_AUTO,
-		.def = V4L2_ISO_SENSITIVITY_AUTO,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_ISO,
-		.setter = ctrl_set_iso,
-	},
-	{
-		.id = V4L2_CID_IMAGE_STABILIZATION,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 1,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_VIDEO_STABILISATION,
-		.setter = ctrl_set_value,
-	},
-	{
-		.id = V4L2_CID_EXPOSURE_AUTO,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~0x03,
-		.max = V4L2_EXPOSURE_APERTURE_PRIORITY,
-		.def = V4L2_EXPOSURE_AUTO,
-		.step = 0,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_EXPOSURE_MODE,
-		.setter = ctrl_set_exposure,
-	},
-	{
-		.id = V4L2_CID_EXPOSURE_ABSOLUTE,
-		.type = MMAL_CONTROL_TYPE_STD,
-		/* Units of 100usecs */
-		.min = 1,
-		.max = 1 * 1000 * 10,
-		.def = 100 * 10,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_SHUTTER_SPEED,
-		.setter = ctrl_set_exposure,
-	},
-	{
-		.id = V4L2_CID_AUTO_EXPOSURE_BIAS,
-		.type = MMAL_CONTROL_TYPE_INT_MENU,
-		.min = 0,
-		.max = ARRAY_SIZE(ev_bias_qmenu) - 1,
-		.def = (ARRAY_SIZE(ev_bias_qmenu) + 1) / 2 - 1,
-		.step = 0,
-		.imenu = ev_bias_qmenu,
-		.mmal_id = MMAL_PARAMETER_EXPOSURE_COMP,
-		.setter = ctrl_set_value_ev,
-	},
-	{
-		.id = V4L2_CID_EXPOSURE_AUTO_PRIORITY,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 1,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		/* Dummy MMAL ID as it gets mapped into FPS range */
-		.mmal_id = 0,
-		.setter = ctrl_set_exposure,
-	},
-	{
-		.id = V4L2_CID_EXPOSURE_METERING,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~0xf,
-		.max = V4L2_EXPOSURE_METERING_MATRIX,
-		.def = V4L2_EXPOSURE_METERING_AVERAGE,
-		.step = 0,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_EXP_METERING_MODE,
-		.setter = ctrl_set_metering_mode,
-	},
-	{
-		.id = V4L2_CID_AUTO_N_PRESET_WHITE_BALANCE,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~0x3ff,
-		.max = V4L2_WHITE_BALANCE_SHADE,
-		.def = V4L2_WHITE_BALANCE_AUTO,
-		.step = 0,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_AWB_MODE,
-		.setter = ctrl_set_awb_mode,
-	},
-	{
-		.id = V4L2_CID_RED_BALANCE,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 1,
-		.max = 7999,
-		.def = 1000,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS,
-		.setter = ctrl_set_awb_gains,
-	},
-	{
-		.id = V4L2_CID_BLUE_BALANCE,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 1,
-		.max = 7999,
-		.def = 1000,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_CUSTOM_AWB_GAINS,
-		.setter = ctrl_set_awb_gains,
-	},
-	{
-		.id = V4L2_CID_COLORFX,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = 0,
-		.max = V4L2_COLORFX_SET_CBCR,
-		.def = V4L2_COLORFX_NONE,
-		.step = 0,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_IMAGE_EFFECT,
-		.setter = ctrl_set_image_effect,
-	},
-	{
-		.id = V4L2_CID_COLORFX_CBCR,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 0xffff,
-		.def = 0x8080,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_COLOUR_EFFECT,
-		.setter = ctrl_set_colfx,
-	},
-	{
-		.id = V4L2_CID_ROTATE,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 360,
-		.def = 0,
-		.step = 90,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_ROTATION,
-		.setter = ctrl_set_rotate,
-	},
-	{
-		.id = V4L2_CID_HFLIP,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 1,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_MIRROR,
-		.setter = ctrl_set_flip,
-	},
-	{
-		.id = V4L2_CID_VFLIP,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 1,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_MIRROR,
-		.setter = ctrl_set_flip,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = 0,
-		.max = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
-		.def = 0,
-		.step = 0,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_RATECONTROL,
-		.setter = ctrl_set_bitrate_mode,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_BITRATE,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 25 * 1000,
-		.max = 25 * 1000 * 1000,
-		.def = 10 * 1000 * 1000,
-		.step = 25 * 1000,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_VIDEO_BIT_RATE,
-		.setter = ctrl_set_bitrate,
-	},
-	{
-		.id = V4L2_CID_JPEG_COMPRESSION_QUALITY,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 1,
-		.max = 100,
-		.def = 30,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_JPEG_Q_FACTOR,
-		.setter = ctrl_set_image_encode_output,
-	},
-	{
-		.id = V4L2_CID_POWER_LINE_FREQUENCY,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = 0,
-		.max = V4L2_CID_POWER_LINE_FREQUENCY_AUTO,
-		.def = 1,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_FLICKER_AVOID,
-		.setter = ctrl_set_flicker_avoidance,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_REPEAT_SEQ_HEADER,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 1,
-		.def = 0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_VIDEO_ENCODE_INLINE_HEADER,
-		.setter = ctrl_set_video_encode_param_output,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~(BIT(V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE) |
-			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_CONSTRAINED_BASELINE) |
-			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_MAIN) |
-			 BIT(V4L2_MPEG_VIDEO_H264_PROFILE_HIGH)),
-		.max = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
-		.def = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_PROFILE,
-		.setter = ctrl_set_video_encode_profile_level,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		.min = ~(BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_0) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1B) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_1) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_2) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_1_3) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_0) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_1) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_2_2) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_0) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_1) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_3_2) |
-			 BIT(V4L2_MPEG_VIDEO_H264_LEVEL_4_0)),
-		.max = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
-		.def = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_PROFILE,
-		.setter = ctrl_set_video_encode_profile_level,
-	},
-	{
-		.id = V4L2_CID_SCENE_MODE,
-		.type = MMAL_CONTROL_TYPE_STD_MENU,
-		/* mask is computed at runtime */
-		.min = -1,
-		.max = V4L2_SCENE_MODE_TEXT,
-		.def = V4L2_SCENE_MODE_NONE,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_PROFILE,
-		.setter = ctrl_set_scene_mode,
-	},
-	{
-		.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
-		.type = MMAL_CONTROL_TYPE_STD,
-		.min = 0,
-		.max = 0x7FFFFFFF,
-		.def = 60,
-		.step = 1,
-		.imenu = NULL,
-		.mmal_id = MMAL_PARAMETER_INTRAPERIOD,
-		.setter = ctrl_set_video_encode_param_output,
-	},
-};
-
-int bcm2835_mmal_set_all_camera_controls(struct bcm2835_mmal_dev *dev)
-{
-	int c;
-	int ret = 0;
-
-	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
-		if ((dev->ctrls[c]) && (v4l2_ctrls[c].setter)) {
-			ret = v4l2_ctrls[c].setter(dev, dev->ctrls[c],
-						   &v4l2_ctrls[c]);
-			if (ret) {
-				v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-					 "Failed when setting default values for ctrl %d\n",
-					 c);
-				break;
-			}
-		}
-	}
-	return ret;
-}
-
-int set_framerate_params(struct bcm2835_mmal_dev *dev)
-{
-	struct mmal_parameter_fps_range fps_range;
-	int ret;
-
-	fps_range.fps_high.numerator = dev->capture.timeperframe.denominator;
-	fps_range.fps_high.denominator = dev->capture.timeperframe.numerator;
-
-	if ((dev->exposure_mode_active != MMAL_PARAM_EXPOSUREMODE_OFF) &&
-	    (dev->exp_auto_priority)) {
-		/* Variable FPS. Define min FPS as 1fps. */
-		fps_range.fps_low.numerator = 1;
-		fps_range.fps_low.denominator = 1;
-	} else {
-		/* Fixed FPS - set min and max to be the same */
-		fps_range.fps_low.numerator = fps_range.fps_high.numerator;
-		fps_range.fps_low.denominator = fps_range.fps_high.denominator;
-	}
-
-	v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
-		 "Set fps range to %d/%d to %d/%d\n",
-		 fps_range.fps_low.numerator,
-		 fps_range.fps_low.denominator,
-		 fps_range.fps_high.numerator,
-		 fps_range.fps_high.denominator);
-
-	ret = vchiq_mmal_port_parameter_set(dev->instance,
-					    &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW],
-					    MMAL_PARAMETER_FPS_RANGE,
-					    &fps_range, sizeof(fps_range));
-	ret += vchiq_mmal_port_parameter_set(dev->instance,
-					     &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO],
-					     MMAL_PARAMETER_FPS_RANGE,
-					     &fps_range, sizeof(fps_range));
-	ret += vchiq_mmal_port_parameter_set(dev->instance,
-					     &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE],
-					     MMAL_PARAMETER_FPS_RANGE,
-					     &fps_range, sizeof(fps_range));
-	if (ret)
-		v4l2_dbg(0, bcm2835_v4l2_debug, &dev->v4l2_dev,
-			 "Failed to set fps ret %d\n", ret);
-
-	return ret;
-}
-
-int bcm2835_mmal_init_controls(struct bcm2835_mmal_dev *dev, struct v4l2_ctrl_handler *hdl)
-{
-	int c;
-	const struct bcm2835_mmal_v4l2_ctrl *ctrl;
-
-	v4l2_ctrl_handler_init(hdl, V4L2_CTRL_COUNT);
-
-	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
-		ctrl = &v4l2_ctrls[c];
-
-		switch (ctrl->type) {
-		case MMAL_CONTROL_TYPE_STD:
-			dev->ctrls[c] = v4l2_ctrl_new_std(hdl, &bcm2835_mmal_ctrl_ops,
-							  ctrl->id, ctrl->min, ctrl->max,
-							  ctrl->step, ctrl->def);
-			break;
-
-		case MMAL_CONTROL_TYPE_STD_MENU:
-		{
-			u64 mask = ctrl->min;
-
-			if (ctrl->id == V4L2_CID_SCENE_MODE) {
-				/* Special handling to work out the mask
-				 * value based on the scene_configs array
-				 * at runtime. Reduces the chance of
-				 * mismatches.
-				 */
-				int i;
-
-				mask = BIT(V4L2_SCENE_MODE_NONE);
-				for (i = 0;
-				     i < ARRAY_SIZE(scene_configs);
-				     i++) {
-					mask |= BIT(scene_configs[i].v4l2_scene);
-				}
-				mask = ~mask;
-			}
-
-			dev->ctrls[c] = v4l2_ctrl_new_std_menu(hdl, &bcm2835_mmal_ctrl_ops,
-							       ctrl->id, ctrl->max, mask,
-							       ctrl->def);
-			break;
-		}
-
-		case MMAL_CONTROL_TYPE_INT_MENU:
-			dev->ctrls[c] = v4l2_ctrl_new_int_menu(hdl, &bcm2835_mmal_ctrl_ops,
-							       ctrl->id, ctrl->max,
-							       ctrl->def, ctrl->imenu);
-			break;
-
-		case MMAL_CONTROL_TYPE_CLUSTER:
-			/* skip this entry when constructing controls */
-			continue;
-		}
-
-		if (hdl->error)
-			break;
-
-		dev->ctrls[c]->priv = (void *)ctrl;
-	}
-
-	if (hdl->error) {
-		pr_err("error adding control %d/%d id 0x%x\n", c,
-		       V4L2_CTRL_COUNT, ctrl->id);
-		return hdl->error;
-	}
-
-	for (c = 0; c < V4L2_CTRL_COUNT; c++) {
-		ctrl = &v4l2_ctrls[c];
-
-		switch (ctrl->type) {
-		case MMAL_CONTROL_TYPE_CLUSTER:
-			v4l2_ctrl_auto_cluster(ctrl->min,
-					       &dev->ctrls[c + 1],
-					       ctrl->max,
-					       ctrl->def);
-			break;
-
-		case MMAL_CONTROL_TYPE_STD:
-		case MMAL_CONTROL_TYPE_STD_MENU:
-		case MMAL_CONTROL_TYPE_INT_MENU:
-			break;
-		}
-	}
-
-	return 0;
-}
diff --git a/drivers/staging/vc04_services/interface/TODO b/drivers/staging/vc04_services/interface/TODO
deleted file mode 100644
index f6f24600aa86..000000000000
--- a/drivers/staging/vc04_services/interface/TODO
+++ /dev/null
@@ -1,28 +0,0 @@
-* Import drivers using VCHI.
-
-VCHI is just a tool to let drivers talk to the firmware.  Here are
-some of the ones we want:
-
-  - vc_mem (https://github.com/raspberrypi/linux/blob/rpi-4.4.y/drivers/char/broadcom/vc_mem.c)
-
-  This driver is what the vcdbg userspace program uses to set up its
-  requests to the firmware, which are transmitted across VCHIQ.  vcdbg
-  is really useful for debugging firmware interactions.
-
-  - VCSM (https://github.com/raspberrypi/linux/tree/rpi-4.4.y/drivers/char/broadcom/vc_sm)
-
-  This driver is used for talking about regions of VC memory across
-  firmware protocols including VCHI.  We'll want to extend this driver
-  to manage these buffers as dmabufs so that we can zero-copy import
-  camera images into vc4 for rendering/display.
-
-* Documentation
-
-A short top-down description of this driver's architecture (function of
-kthreads, userspace, limitations) could be very helpful for reviewers.
-
-* Reformat core code with more sane indentations
-
-The code follows the 80 characters limitation yet tends to go 3 or 4 levels of
-indentation deep making it very unpleasant to read. This is specially relevant
-in the character driver ioctl code and in the core thread functions.
diff --git a/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h b/include/linux/raspberrypi/vchiq.h
index ee4469f4fc51..ee4469f4fc51 100644
--- a/drivers/staging/vc04_services/include/linux/raspberrypi/vchiq.h
+++ b/include/linux/raspberrypi/vchiq.h
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h b/include/linux/raspberrypi/vchiq_arm.h
index e32b02f99024..e32b02f99024 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.h
+++ b/include/linux/raspberrypi/vchiq_arm.h
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_bus.h b/include/linux/raspberrypi/vchiq_bus.h
index 9de179b39f85..9de179b39f85 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_bus.h
+++ b/include/linux/raspberrypi/vchiq_bus.h
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h b/include/linux/raspberrypi/vchiq_cfg.h
index a16d0299996c..a16d0299996c 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_cfg.h
+++ b/include/linux/raspberrypi/vchiq_cfg.h
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h b/include/linux/raspberrypi/vchiq_core.h
index 9b4e766990a4..e7bf7a114985 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.h
+++ b/include/linux/raspberrypi/vchiq_core.h
@@ -15,7 +15,7 @@
 #include <linux/spinlock_types.h>
 #include <linux/wait.h>
 
-#include "../../include/linux/raspberrypi/vchiq.h"
+#include "vchiq.h"
 #include "vchiq_cfg.h"
 
 /* Do this so that we can test-build the code on non-rpi systems */
@@ -171,6 +171,21 @@ struct vchiq_slot_info {
 	short release_count;
 };
 
+/*
+ * VCHIQ is a reliable connection-oriented datagram protocol.
+ *
+ * A VCHIQ service is equivalent to a TCP connection, except:
+ * + FOURCCs are used for the rendezvous, and port numbers are assigned at the
+ *   time the connection is established.
+ * + There is less of a distinction between server and client sockets, the only
+ *   difference being which end makes the first move.
+ * + For a multi-client server, the server creates new "listening" services as
+ *   the existing one becomes connected - there is no need to specify the
+ *   maximum number of clients up front.
+ * + Data transfer is reliable but packetized (messages have defined ends).
+ * + Messages can be either short (capable of fitting in a slot) and in-band,
+ *   or copied between external buffers (bulk transfers).
+ */
 struct vchiq_service {
 	struct vchiq_service_base base;
 	unsigned int handle;
@@ -286,6 +301,23 @@ struct vchiq_shared_state {
 	int debug[DEBUG_MAX];
 };
 
+/*
+ * vchiq_slot_zero describes the memory shared between the ARM host and the
+ * VideoCore VPU. The "master" and "slave" states are owned by the respective
+ * sides but visible to the other; the slots are shared, and the remaining
+ * fields are read-only.
+ *
+ * In the configuration used by this implementation, the memory is allocated
+ * by the host, the VPU is the master (the side which controls the DMA for bulk
+ * transfers), and the host is the slave.
+ *
+ * The ownership of slots changes with use:
+ * + When empty they are owned by the sender.
+ * + When partially filled they are shared with the receiver.
+ * + When completely full they are owned by the receiver.
+ * + When the receiver has finished processing the contents, they are recycled
+ *   back to the sender.
+ */
 struct vchiq_slot_zero {
 	int magic;
 	short version;
@@ -300,6 +332,10 @@ struct vchiq_slot_zero {
 	struct vchiq_slot_info slots[VCHIQ_MAX_SLOTS];
 };
 
+/*
+ * This is the private runtime state used by each side. The same structure was
+ * originally used by both sides, but implementations have since diverged.
+ */
 struct vchiq_state {
 	struct device *dev;
 	int id;
@@ -321,13 +357,27 @@ struct vchiq_state {
 	struct mutex mutex;
 	struct vchiq_instance **instance;
 
-	/* Processes incoming messages */
+	/* Processes all incoming messages which aren't synchronous */
 	struct task_struct *slot_handler_thread;
 
-	/* Processes recycled slots */
+	/*
+	 * Slots which have been fully processed and released by the (peer)
+	 * receiver are added to the receiver queue, which is asynchronously
+	 * processed by the recycle thread.
+	 */
 	struct task_struct *recycle_thread;
 
-	/* Processes synchronous messages */
+	/*
+	 * Processes incoming synchronous messages
+	 *
+	 * The synchronous message channel is shared between all synchronous
+	 * services, and provides a way for urgent messages to bypass
+	 * potentially long queues of asynchronous messages in the normal slots.
+	 *
+	 * There can be only one outstanding synchronous message in
+	 * each direction, and as a precious shared resource synchronous
+	 * services should be used sparingly.
+	 */
 	struct task_struct *sync_thread;
 
 	/* Local implementation of the trigger remote event */
diff --git a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h b/include/linux/raspberrypi/vchiq_debugfs.h
index b29e6693c949..b29e6693c949 100644
--- a/drivers/staging/vc04_services/interface/vchiq_arm/vchiq_debugfs.h
+++ b/include/linux/raspberrypi/vchiq_debugfs.h
diff --git a/drivers/staging/gpib/uapi/gpib.h b/include/uapi/linux/gpib.h
index ddf82a4d989f..2a7f5eeb9777 100644
--- a/drivers/staging/gpib/uapi/gpib.h
+++ b/include/uapi/linux/gpib.h
@@ -1,4 +1,4 @@
-/* SPDX-License-Identifier: GPL-2.0 */
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
 
 /***************************************************************************
  *    copyright		   : (C) 2002 by Frank Mori Hess
diff --git a/drivers/staging/gpib/uapi/gpib_ioctl.h b/include/uapi/linux/gpib_ioctl.h
index 55bf5e55507a..d544d8e4362c 100644
--- a/drivers/staging/gpib/uapi/gpib_ioctl.h
+++ b/include/uapi/linux/gpib_ioctl.h
@@ -1,4 +1,4 @@
-/* SPDX-License-Identifier: GPL-2.0 */
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
 
 /***************************************************************************
  *    copyright            : (C) 2002 by Frank Mori Hess
@@ -40,7 +40,7 @@ struct gpib_serial_poll_ioctl {
 	__u32 pad;
 	__s32 sad;
 	__u8 status_byte;
-	__u8 padding[3];   // align to 32 bit boundary
+	__u8 padding[3];   /* align to 32 bit boundary */
 };
 
 struct gpib_eos_ioctl {
@@ -80,7 +80,7 @@ struct gpib_board_info_ioctl {
 	__u32 t1_delay;
 	unsigned ist : 1;
 	unsigned no_7_bit_eos : 1;
-	unsigned padding :30; // align to 32 bit boundary
+	unsigned padding :30; /* align to 32 bit boundary */
 };
 
 struct gpib_select_pci_ioctl {
@@ -92,7 +92,7 @@ struct gpib_ppoll_config_ioctl {
 	__u8 config;
 	unsigned set_ist : 1;
 	unsigned clear_ist : 1;
-	unsigned padding :22; // align to 32 bit boundary
+	unsigned padding :22; /* align to 32 bit boundary */
 };
 
 struct gpib_pad_ioctl {
@@ -105,15 +105,15 @@ struct gpib_sad_ioctl {
 	__s32 sad;
 };
 
-// select a piece of hardware to attach by its sysfs device path
+/* select a piece of hardware to attach by its sysfs device path */
 struct gpib_select_device_path_ioctl {
 	char device_path[0x1000];
 };
 
-// update status byte and request service
+/* update status byte and request service */
 struct gpib_request_service2 {
 	__u8 status_byte;
-	__u8 padding[3]; // align to 32 bit boundary
+	__u8 padding[3]; /* align to 32 bit boundary */
 	__s32 new_reason_for_service;
 };
 
@@ -160,7 +160,7 @@ enum gpib_ioctl {
 	IBPP2_SET = _IOW(GPIB_CODE, 40, __s16),
 	IBPP2_GET = _IOR(GPIB_CODE, 41, __s16),
 	IBSELECT_DEVICE_PATH = _IOW(GPIB_CODE, 43, struct gpib_select_device_path_ioctl),
-	// 44 was IBSELECT_SERIAL_NUMBER
+	/* 44 was IBSELECT_SERIAL_NUMBER */
 	IBRSV2 = _IOW(GPIB_CODE, 45, struct gpib_request_service2)
 };