diff --git a/Documentation/ABI/testing/sysfs-kernel-iommu_groups b/Documentation/ABI/testing/sysfs-kernel-iommu_groups
new file mode 100644
index 0000000..9b31556
--- /dev/null
+++ b/Documentation/ABI/testing/sysfs-kernel-iommu_groups
@@ -0,0 +1,14 @@
+What:		/sys/kernel/iommu_groups/
+Date:		May 2012
+KernelVersion:	v3.5
+Contact:	Alex Williamson <alex.williamson@redhat.com>
+Description:	/sys/kernel/iommu_groups/ contains a number of sub-
+		directories, each representing an IOMMU group.  The
+		name of the sub-directory matches the iommu_group_id()
+		for the group, which is an integer value.  Within each
+		subdirectory is another directory named "devices" with
+		links to the sysfs devices contained in this group.
+		The group directory also optionally contains a "name"
+		file if the IOMMU driver has chosen to register a more
+		common name for the group.
+Users:
diff --git a/Documentation/devicetree/bindings/arm/msm/dcvs-core-info.txt b/Documentation/devicetree/bindings/arm/msm/dcvs-core-info.txt
index b7dd427..f8152cfb 100644
--- a/Documentation/devicetree/bindings/arm/msm/dcvs-core-info.txt
+++ b/Documentation/devicetree/bindings/arm/msm/dcvs-core-info.txt
@@ -37,9 +37,9 @@
 - qcom,algo-ss-win-size-max-us:		sets maximum steady state window size.
 - qcom,algo-ss-util-pct:		sets target CPU utilization during
 					steady-state.
-- qcom,algo-ss-iobusy-conv:		specifies how wait time (i/o busy time)
-					is incorporated into the steady-state
-					algorithm.
+- qcom,algo-ss-no-corr-below-freq:	specifies frequency below which DCVS
+					will not attempt to correlate busy or
+					idle information from different CPUs
 
 - qcom,energy-active-coeff-a:	sets active power equation coefficient a.
 - qcom,energy-active-coeff-b:	sets active power equation coefficient b.
@@ -89,7 +89,7 @@
 			qcom,algo-ss-win-size-min-us = <1000000>;
 			qcom,algo-ss-win-size-max-us = <1000000>;
 			qcom,algo-ss-util-pct = <95>;
-			qcom,algo-ss-iobusy-conv = <100>;
+			qcom,algo-ss-no-corr-below-freq = <0>;
 
 			qcom,energy-active-coeff-a = <2492>;
 			qcom,energy-active-coeff-b = <0>;
diff --git a/Documentation/devicetree/bindings/arm/msm/lpm-resources.txt b/Documentation/devicetree/bindings/arm/msm/lpm-resources.txt
index 7f2a21b..ccb3465 100644
--- a/Documentation/devicetree/bindings/arm/msm/lpm-resources.txt
+++ b/Documentation/devicetree/bindings/arm/msm/lpm-resources.txt
@@ -23,6 +23,7 @@
 - qcom,resource-type: The type of the LPM resource.
    MSM_LPM_RPM_RS_TYPE    = 0
    MSM_LPM_LOCAL_RS_TYPE  = 1
+- qcom,init-value: Initialization value of the LPM resource.
 
 
 Optional Nodes:
@@ -41,5 +42,6 @@
                         qcom,type = <0x62706d73>;   /* "smpb" */
                         qcom,id = <0x02>;
                         qcom,key = <0x6e726f63>;   /* "corn" */
+			qcom,init-value= <5>;   /* Active Corner*/
                 };
 
diff --git a/Documentation/devicetree/bindings/ata/ahci-platform.txt b/Documentation/devicetree/bindings/ata/ahci-platform.txt
index 8bb8a76..3e309e4 100644
--- a/Documentation/devicetree/bindings/ata/ahci-platform.txt
+++ b/Documentation/devicetree/bindings/ata/ahci-platform.txt
@@ -4,7 +4,8 @@
 Each SATA controller should have its own node.
 
 Required properties:
-- compatible        : compatible list, contains "calxeda,hb-ahci" or "snps,spear-ahci"
+- compatible        : compatible list, contains "calxeda,hb-ahci" or
+		      "snps,spear-ahci" or "qcom,msm-ahci"
 - interrupts        : <interrupt mapping for SATA IRQ>
 - reg               : <registers mapping>
 
diff --git a/Documentation/devicetree/bindings/fb/msm-hdmi-tx.txt b/Documentation/devicetree/bindings/fb/msm-hdmi-tx.txt
index e777094..a30d1d6 100644
--- a/Documentation/devicetree/bindings/fb/msm-hdmi-tx.txt
+++ b/Documentation/devicetree/bindings/fb/msm-hdmi-tx.txt
@@ -6,6 +6,7 @@
 - reg: offset and length of the register regions(s) for the device.
 - reg-names: a list of strings that map in order to the list of regs.
 
+- hpd-gdsc-supply: phandle to the mdss gdsc regulator device tree node.
 - hpd-5v-supply: phandle to the 5V regulator device tree node.
 - core-vdda-supply: phandle to the HDMI vdda regulator device tree node.
 - core-vcc-supply: phandle to the HDMI vcc regulator device tree node.
@@ -33,6 +34,14 @@
 - qcom,hdmi-tx-mux-en: gpio required to enable mux for HDMI output
   on liquid devices. Required property for liquid devices.
 
+[Optional child nodes]: These nodes are for devices which are
+dependent on HDMI Tx controller. If HDMI Tx controller is disabled then
+these devices will be disabled as well. Ex. HDMI Audio Codec device.
+
+- qcom,msm-hdmi-audio-rx: Node for HDMI audio codec.
+Required properties:
+- compatible : "msm-hdmi-audio-codec-rx";
+
 Example:
 	qcom,hdmi_tx@fd922100 {
 		cell-index = <0>;
@@ -42,17 +51,22 @@
 			<0xfc4b8000 0x60F0>;
 		reg-names = "core_physical", "phy_physical", "qfprom_physical";
 
+		hpd-gdsc-supply = <&gdsc_mdss>;
 		hpd-5v-supply = <&pm8941_mvs2>;
 		core-vdda-supply = <&pm8941_l12>;
 		core-vcc-supply = <&pm8941_s3>;
-		qcom,hdmi-tx-supply-names = "hpd-5v", "core-vdda", "core-vcc";
-		qcom,hdmi-tx-supply-type = <0 1 1>;
-		qcom,hdmi-tx-min-voltage-level = <0 1800000 1800000>;
-		qcom,hdmi-tx-max-voltage-level = <0 1800000 1800000>;
-		qcom,hdmi-tx-op-mode = <0 1800000 0>;
+		qcom,hdmi-tx-supply-names = "hpd-gdsc", "hpd-5v", "core-vdda", "core-vcc";
+		qcom,hdmi-tx-supply-type = <1 1 0 0>;
+		qcom,hdmi-tx-min-voltage-level = <0 0 1800000 1800000>;
+		qcom,hdmi-tx-max-voltage-level = <0 0 1800000 1800000>;
+		qcom,hdmi-tx-op-mode = <0 0 1800000 0>;
 
 		qcom,hdmi-tx-cec = <&msmgpio 31 0>;
 		qcom,hdmi-tx-ddc-clk = <&msmgpio 32 0>;
 		qcom,hdmi-tx-ddc-data = <&msmgpio 33 0>;
 		qcom,hdmi-tx-hpd = <&msmgpio 34 0>;
+
+		qcom,msm-hdmi-audio-rx {
+			compatible = "qcom,msm-hdmi-audio-codec-rx";
+		};
 	};
diff --git a/Documentation/devicetree/bindings/gpio/gpio-msm.txt b/Documentation/devicetree/bindings/gpio/gpio-msm.txt
index 359d700..a5d50fd 100644
--- a/Documentation/devicetree/bindings/gpio/gpio-msm.txt
+++ b/Documentation/devicetree/bindings/gpio/gpio-msm.txt
@@ -9,6 +9,7 @@
 - gpio-controller : Marks the device node as a GPIO controller.
 - #interrupt-cells : Should be 2.
 - interrupt-controller: Mark the device node as an interrupt controller
+- interrupts : Specify the TLMM summary interrupt number
 
 Example:
 
@@ -19,6 +20,7 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		interrupts = <0 208 0>;
 	};
 
 To specify gpios for a device:
diff --git a/Documentation/devicetree/bindings/gpu/adreno.txt b/Documentation/devicetree/bindings/gpu/adreno.txt
index 9164647..2ea9ba9 100644
--- a/Documentation/devicetree/bindings/gpu/adreno.txt
+++ b/Documentation/devicetree/bindings/gpu/adreno.txt
@@ -132,7 +132,7 @@
 			qcom,algo-ss-window-size = <1000000>;
 			qcom,algo-ss-util-pct = <95>;
 			qcom,algo-em-max-util-pct = <97>;
-			qcom,algo-ss-iobusy-conv = <100>;
+			qcom,algo-ss-no-corr-below-freq = <0>;
 
 			qcom,dcvs-freq@0 {
 				reg = <0>;
diff --git a/Documentation/devicetree/bindings/hwmon/qpnp-adc-current.txt b/Documentation/devicetree/bindings/hwmon/qpnp-adc-current.txt
index e458ea0..fbe8ffa 100644
--- a/Documentation/devicetree/bindings/hwmon/qpnp-adc-current.txt
+++ b/Documentation/devicetree/bindings/hwmon/qpnp-adc-current.txt
@@ -10,6 +10,8 @@
 Required properties:
 - compatible : should be "qcom,qpnp-iadc" for Current ADC driver.
 - reg : offset and length of the PMIC Aribter register map.
+- address-cells : Must be one.
+- size-cells : Must be zero.
 - interrupts : The USR bank peripheral IADC interrupt.
 - interrupt-names : Should contain "eoc-int-en-set".
 - qcom,adc-bit-resolution : Bit resolution of the ADC.
@@ -21,6 +23,7 @@
 
 Required properties:
 - label : Channel name used for sysfs entry.
+- reg : AMUX channel number.
 - qcom,channel-num : Channel number associated to the AMUX input.
 - qcom,decimation : Sampling rate to use for the individual channel measurement.
 		    Select from the following unsigned int.
@@ -84,6 +87,8 @@
 	qcom,iadc@3200 {
                         compatible = "qcom,qpnp-iadc";
                         reg = <0x3200 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
                         interrupts = <0 0x36 0>;
 			interrupt-names = "eoc-int-en-set";
                         qcom,adc-bit-resolution = <16>;
@@ -93,7 +98,7 @@
 			/* Channel Node */
                         chan@0 = {
                                 label = "rsense";
-                                qcom,channel-num = <0>;
+                                reg = <0>;
                                 qcom,decimation = <0>;
                                 qcom,pre-div-channel-scaling = <20>;
                                 qcom,calibration-type = "fresh";
diff --git a/Documentation/devicetree/bindings/hwmon/qpnp-adc-voltage.txt b/Documentation/devicetree/bindings/hwmon/qpnp-adc-voltage.txt
index e23605c..bb66e7b 100644
--- a/Documentation/devicetree/bindings/hwmon/qpnp-adc-voltage.txt
+++ b/Documentation/devicetree/bindings/hwmon/qpnp-adc-voltage.txt
@@ -10,6 +10,8 @@
 Required properties:
 - compatible : should be "qcom,qpnp-vadc" for Voltage ADC driver.
 - reg : offset and length of the PMIC Aribter register map.
+- address-cells : Must be one.
+- size-cells : Must be zero.
 - interrupts : The USR bank peripheral VADC interrupt.
 - interrupt-names : Should contain "eoc-int-en-set".
 - qcom,adc-bit-resolution : Bit resolution of the ADC.
@@ -20,7 +22,7 @@
 
 Required properties:
 - label : Channel name used for sysfs entry.
-- qcom,channel-num : Channel number associated to the AMUX input.
+- reg : AMUX channel number.
 - qcom,decimation : Sampling rate to use for the individual channel measurement.
 		    Select from following unsigned int.
 		    0 : 512
@@ -82,6 +84,8 @@
 	qcom,vadc@3100 {
                         compatible = "qcom,qpnp-vadc";
                         reg = <0x3100 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
                         interrupts = <0x0 0x31 0x0>;
 			interrupt-names = "eoc-int-en-set";
                         qcom,adc-bit-resolution = <15>;
@@ -90,7 +94,7 @@
 			/* Channel Node */
                         chan@0 {
                                 label = "usb_in";
-                                qcom,channel-num = <0>;
+                                reg = <0>;
                                 qcom,decimation = <0>;
                                 qcom,pre-div-channel-scaling = <20>;
                                 qcom,calibration-type = "absolute";
diff --git a/Documentation/devicetree/bindings/iommu/msm_iommu_v1.txt b/Documentation/devicetree/bindings/iommu/msm_iommu_v1.txt
new file mode 100644
index 0000000..d8e7791
--- /dev/null
+++ b/Documentation/devicetree/bindings/iommu/msm_iommu_v1.txt
@@ -0,0 +1,37 @@
+* Qualcomm MSM IOMMU v1
+
+Required properties:
+- compatible : one of:
+	- "qcom,msm-smmu-v1"
+- reg : offset and length of the register set for the device.
+- qcom,glb-offset : Offset for the global register base.
+
+Optional properties:
+- List of sub nodes, one for each of the translation context banks supported.
+  Each sub node has the following required properties:
+
+  - reg : offset and length of the register set for the context bank.
+  - interrupts : should contain the context bank interrupt.
+  - qcom,iommu-ctx-mids : List of machine identifiers associated with this
+    translation context.
+  - label : Name of the context bank
+
+Optional properties:
+  - none
+
+Example:
+
+	qcom,iommu@fd000000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd890000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+
+		qcom,iommu-ctx@fd000000 {
+			reg = <0xfd000000 0x1000>;
+			interrupts = <0 250 0>;
+			qcom,iommu-ctx-mids = <0 3>;
+			label = "a_label";
+		};
diff --git a/Documentation/devicetree/bindings/mmc/msm_sdcc.txt b/Documentation/devicetree/bindings/mmc/msm_sdcc.txt
index 46173a0..b99b716 100644
--- a/Documentation/devicetree/bindings/mmc/msm_sdcc.txt
+++ b/Documentation/devicetree/bindings/mmc/msm_sdcc.txt
@@ -4,61 +4,74 @@
 SD/MMC/SDIO cards.
 
 Required properties:
-  - compatible : should be "qcom,msm-sdcc"
-  - reg : should contain SDCC, SDCC-DML and BAM register map.
-  - reg-names : indicates various resources passed to driver (via reg proptery) by name.
-  "reg-names" examples are "core_mem", "dml_mem" and "bam_mem"
-  - interrupts : should contain SDCC core interrupt.
-  - interrupt-names : indicates interrupts passed to driver (via interrupts property) by name.
-  "core_irq" is mandatory, "bam_irq" is mandatory only when BAM DMA engine is used,
-  "status_irq" and "sdiowakeup_irq" are optional.
-  - qcom,clk-rates : specifies supported SDCC clock frequencies, Units - Hz.
-  - qcom,sup-voltages: specifies supported voltage ranges for card. Should always be
-			specified in pairs (min, max), Units - mV.
-  - <supply-name>-supply: phandle to the regulator device tree node
-  "supply-name" examples are "vdd", "vdd-io".
+  - compatible: should be "qcom,msm-sdcc"
+  - reg: should contain SDCC (mandatory), SDCC-DML (optional) and BAM
+    (optional) register maps.
+  - reg-names: indicates various resources passed to driver (via reg
+    property) by name. "reg-names" examples are "core_mem", "dml_mem"
+    and "bam_mem". "dml_mem" and "bam_mem" are optional, and the SDCC
+    driver will default to PIO mode when neither are present.
+  - interrupts: should contain SDCC core interrupt.
+  - interrupt-names: indicates interrupts passed to driver (via interrupts
+    property) by name. "core_irq" is mandatory, "bam_irq" is mandatory only
+    when BAM DMA engine is used. "status_irq" and "sdiowakeup_irq" are
+    optional.
+  - qcom,clk-rates: specifies supported SDCC clock frequencies, Units - Hz.
+  - qcom,sup-voltages: specifies supported voltage ranges for card. Should
+    always be specified in pairs (min, max), Units - mV.
+  - <supply-name>-supply: phandle to the regulator device tree node.
+    "supply-name" examples are "vdd", "vdd-io".
 
 Optional Properties:
-	- cell-index - defines slot ID.
-	- qcom,bus-width - defines the bus I/O width that controller supports.
-	- wp-gpios - specify GPIO for write protect switch detection.
-	- cd-gpios - specify GPIO for card detection.
-	- qcom,nonremovable - specifies whether the card in slot is
-				hot pluggable or hard wired.
-	- qcom,disable-cmd23 - disable sending CMD23 to card when controller can't support it.
-	- qcom,bus-speed-mode - specifies supported bus speed modes by host.
-	- qcom,current-limit - specifies max. current the host can drive.
-	- qcom,xpc - specifies if the host can supply more than 150mA for SDXC cards.
-	- qcom,dat1-mpm-int - specifies MPM interrupt number corresponding to DAT1 line of SDCC
-					(used only if slot has dedicated DAT1 MSM pin (not GPIO))
+  - cell-index: defines slot ID.
+  - qcom,bus-width: defines the bus I/O width that controller supports.
+  - wp-gpios: specify GPIO for write protect switch detection.
+  - cd-gpios: specify GPIO for card detection.
+  - qcom,nonremovable: specifies whether the card in slot is hot pluggable
+    or hard wired.
+  - qcom,disable-cmd23: disable sending CMD23 to card when controller
+    can't support it.
+  - qcom,bus-speed-mode: specifies supported bus speed modes by host.
+  - qcom,current-limit: specifies max. current the host can drive.
+  - qcom,xpc: specifies if the host can supply more than 150mA for SDXC
+    cards.
+  - qcom,dat1-mpm-int: specifies MPM interrupt number corresponding to
+    DAT1 line of SDCC (used only if slot has dedicated DAT1 MSM pin
+    (not GPIO))
 
-In the following, <supply> can be vdd (flash core voltage) or vdd-io (I/O voltage).
-	- qcom,<supply>-always-on - specifies whether supply should be kept "on" always.
-	- qcom,<supply>-lpm-sup - specifies whether supply can be kept in low power mode (lpm).
-	- qcom,<supply>-voltage-level - specifies voltage levels for supply. Should be
-	specified in pairs (min, max), units uV.
-	- qcom,<supply>-current-level - specifies load levels for supply in lpm or
-	high power mode (hpm). Should be specified in pairs (lpm, hpm), units uA.
+In the following, <supply> can be vdd (flash core voltage) or vdd-io
+(I/O voltage).
+  - qcom,<supply>-always-on: specifies whether supply should be kept "on"
+    always.
+  - qcom,<supply>-lpm-sup: specifies whether supply can be kept in low
+    power mode (lpm).
+  - qcom,<supply>-voltage-level: specifies voltage levels for supply.
+    Should be specified in pairs (min, max), units uV.
+  - qcom,<supply>-current-level: specifies load levels for supply in
+    lpm or high power mode (hpm). Should be specified in pairs (lpm, hpm),
+    units uA.
 
-	- gpios - specifies gpios assigned for sdcc slot.
-	- qcom,gpio-names -  a list of strings that map in order to the list of gpios
-	A slot has either gpios or dedicated tlmm pins as represented below.
-	- qcom,pad-pull-on - Active pull configuration for sdc tlmm pins
-	- qcom,pad-pull-off - Suspend pull configuration for sdc tlmm pins.
-	- qcom,pad-drv-on - Active drive strength configuration for sdc tlmm pins.
-	- qcom,pad-drv-off - Suspend drive strength configuration for sdc tlmm pins.
-	Tlmm pins are specified as <clk cmd data>
+  - gpios: specifies gpios assigned for sdcc slot.
+  - qcom,gpio-names:  a list of strings that map in order to the list
+    of gpios. A slot has either gpios or dedicated tlmm pins as represented
+    below.
+  - qcom,pad-pull-on: Active pull configuration for sdc tlmm pins
+  - qcom,pad-pull-off: Suspend pull configuration for sdc tlmm pins.
+  - qcom,pad-drv-on: Active drive strength configuration for sdc tlmm pins.
+  - qcom,pad-drv-off: Suspend drive strength configuration for sdc tlmm pins.
+    Tlmm pins are specified as <clk cmd data>
 
-	- qcom,bus-bw-vectors-bps - specifies array of throughput values in Bytes/sec. The
-	values in the array are determined according to supported bus speed modes. For example,
-	if host supports SDR12 mode, value is 13631488 Bytes/sec.
-	- Refer to "Documentation/devicetree/bindings/arm/msm/msm_bus.txt" for
-	below optional properties:
-		- qcom,msm-bus,name
-		- qcom,msm-bus,num-cases
-		- qcom,msm-bus,active-only
-		- qcom,msm-bus,num-paths
-		- qcom,msm-bus,vectors-KBps
+  - qcom,bus-bw-vectors-bps: specifies array of throughput values in
+    Bytes/sec. The values in the array are determined according to
+    supported bus speed modes. For example, if host supports SDR12 mode,
+    value is 13631488 Bytes/sec.
+  - Refer to "Documentation/devicetree/bindings/arm/msm/msm_bus.txt" for
+    below optional properties:
+	- qcom,msm-bus,name
+	- qcom,msm-bus,num-cases
+	- qcom,msm-bus,active-only
+	- qcom,msm-bus,num-paths
+	- qcom,msm-bus,vectors-KBps
 
 Example:
 
@@ -67,6 +80,7 @@
 	cell-index = <1>;
 	compatible = "qcom,msm-sdcc";
 	reg = <0xf9600000 0x800   // SDCC register interface
+	/* To use PIO instead of BAM, skip DML and BAM regs */
 		0xf9600800 0x1800  // DML register interface
 		0xf9602000 0x2000> // BAM register interface
 
diff --git a/Documentation/devicetree/bindings/power/qpnp-bms.txt b/Documentation/devicetree/bindings/power/qpnp-bms.txt
index 4d571eb..d91086f 100644
--- a/Documentation/devicetree/bindings/power/qpnp-bms.txt
+++ b/Documentation/devicetree/bindings/power/qpnp-bms.txt
@@ -4,24 +4,17 @@
 to the battery. It's main function is to calculate the State of Charge (SOC),
 a 0-100 percentage representing the amount of charge left in the battery.
 
-BMS node
+There are two required peripherals in the BMS driver, both implemented as
+subnodes in the example. These peripherals must not be disabled if the BMS
+device is to enabled:
+- qcom,bms-bms : The main BMS device. Supports battery monitoring controls and
+		sensors.
+- qcom,bms-iadc : The BMS IADC peripheral in the IADC device. This is required
+		to determine whether the BMS is using an internal or external
+		rsense to accumulate the Coulomb Counter and read current.
 
-Required properties:
+Parent node required properties:
 - compatible : should be "qcom,qpnp-bms" for the BM driver.
-- reg : offset and length of the PMIC BMS register map.
-- interrupts : the interrupt mappings for bms.
-		The format should be
-		<slave-id peripheral-id interrupt-number>.
-- interrupt-names : names for the mapped bms interrupt
-		The following interrupts are required:
-		0 : vsense_for_r
-		1 : vsense_avg
-		2 : sw_cc_thr
-		3 : ocv_thr
-		4 : charge_begin
-		5 : good_ocv
-		6 : ocv_for_r
-		7 : cc_thr
 - qcom,bms-r-sense-mohm : sensor resistance in in milli-ohms.
 - qcom,bms-v-cutoff-uv : cutoff voltage where the battery is considered dead in
 			micro-volts.
@@ -56,7 +49,7 @@
 			to the enum defined in
 			include/linux/mfd/pm8xxx/batterydata-lib.h
 
-Optional properties:
+Parent node optional properties:
 - qcom,bms-ignore-shutdown-soc: A boolean that controls whether BMS will
 			try to force the startup SoC to be the same as the
 			shutdown SoC. Defining it will make BMS ignore the
@@ -64,15 +57,55 @@
 - qcom,bms-use-voltage-soc : A boolean that controls whether BMS will use
 			voltage-based SoC instead of a coulomb counter based
 			one. Voltage-based SoC will not guarantee linearity.
+- qcom,bms-use-external-rsense : A boolean that controls whether BMS will use
+			an external sensor resistor instead of the default
+			RDS of the batfet.
+
+All sub node required properties:
+- reg : offset and length of the PMIC peripheral register map.
+
+qcom,bms-bms node required properties:
+- interrupts : the interrupt mappings.
+		The format should be
+		<slave-id peripheral-id interrupt-number>.
+- interrupt-names : names for the mapped bms interrupt
+		The following interrupts are required:
+		0 : vsense_for_r
+		1 : vsense_avg
+		2 : sw_cc_thr
+		3 : ocv_thr
+		4 : charge_begin
+		5 : good_ocv
+		6 : ocv_for_r
+		7 : cc_thr
 
 Example:
-	bms@4000 {
-		#address-cells = <1>;
-		#size-cells = <1>;
+pm8941_bms: qcom,bms {
+	spmi-dev-container;
+	compatible = "qcom,qpnp-bms";
+	#address-cells = <1>;
+	#size-cells = <1>;
+	status = "disabled";
 
-		compatible = "qcom,qpnp-bms";
+	qcom,bms-r-sense-mohm = <2>;
+	qcom,bms-v-cutoff-uv = <3400000>;
+	qcom,bms-max-voltage-uv = <4200000>;
+	qcom,bms-r-conn-mohm = <18>;
+	qcom,bms-shutdown-soc-valid-limit = <20>;
+	qcom,bms-adjust-soc-low-threshold = <25>;
+	qcom,bms-adjust-soc-high-threshold = <45>;
+	qcom,bms-low-soc-calculate-soc-threshold = <15>;
+	qcom,bms-low-soc-calculate-soc-ms = <5000>;
+	qcom,bms-calculate-soc-ms = <20000>;
+	qcom,bms-chg-term-ua = <100000>;
+	qcom,bms-batt-type = <0>;
+
+	qcom,bms-iadc@3800 {
+		reg = <0x3800 0x100>;
+	};
+
+	qcom,bms-bms@4000 {
 		reg = <0x4000 0x100>;
-
 		interrupts =	<0x0 0x40 0x0>,
 				<0x0 0x40 0x1>,
 				<0x0 0x40 0x2>,
@@ -90,18 +123,5 @@
 				  "good_ocv",
 				  "ocv_for_r",
 				  "cc_thr";
-
-		qcom,bms-r-sense-mohm = <10>;
-		qcom,bms-v-cutoff-uv = <3400000>;
-		qcom,bms-max-voltage-uv = <4200000>;
-		qcom,bms-r-conn-mohm = <18>;
-		qcom,bms-shutdown-soc-valid-limit = <20>;
-		qcom,bms-adjust-soc-low-threshold = <25>;
-		qcom,bms-adjust-soc-high-threshold = <45>;
-		qcom,bms-low-soc-calculate-soc-threshold = <15>;
-		qcom,bms-low-soc-calculate-soc-ms = <5000>;
-		qcom,bms-calculate-soc-ms = <20000>;
-		qcom,bms-chg-term-ua = <100000>;
-		qcom,bms-batt-type = <0>;
-		qcom,bms-ignore-shutdown-soc;
 	};
+};
diff --git a/Documentation/devicetree/bindings/power/qpnp-charger.txt b/Documentation/devicetree/bindings/power/qpnp-charger.txt
index 2103bbc..59a9fd9 100644
--- a/Documentation/devicetree/bindings/power/qpnp-charger.txt
+++ b/Documentation/devicetree/bindings/power/qpnp-charger.txt
@@ -26,14 +26,20 @@
 - qcom,chg-vddmax-mv:	Target voltage of battery in mV
 - qcom,chg-vddsafe-mv:	Maximum Vdd voltage in mV
 - qcom,chg-vinmin-mv:	Minimum input voltage in mV
+- qcom,chg-vbatdet-mv:	Battery voltage at which charging resumes
 - qcom,chg-ibatmax-ma:	Maximum battery charge current in mA
 - qcom,chg-ibatterm-ma:	Current at which charging is terminated in mA.
+- qcom,chg-ibatsafe-ma:	Safety battery current setting
 
 Parent node optional properties:
-- qcom,chg-charging-disabled:	Set this property to disable charging
-				by default. This can then be overriden
-				writing the the module parameter
-				"charging_disabled".
+- qcom,chg-charging-disabled:		Set this property to disable charging
+					by default. This can then be overriden
+					writing the the module parameter
+					"charging_disabled".
+- qcom,chg-use-default-batt-values:	Set this flag to force reporting of
+					battery temperature of 250 decidegree
+					Celsius, state of charge to be 50%
+					and disable charging.
 
 Sub node required structure:
 - A qcom,chg node must be a child of an SPMI node that has specified
diff --git a/Documentation/devicetree/bindings/qdsp/msm-adsp-sensors.txt b/Documentation/devicetree/bindings/qdsp/msm-adsp-sensors.txt
new file mode 100644
index 0000000..ff0b1ef
--- /dev/null
+++ b/Documentation/devicetree/bindings/qdsp/msm-adsp-sensors.txt
@@ -0,0 +1,19 @@
+* msm-adsp-sensors
+
+Required properties:
+
+ - compatible:	"qcom,msm-adsp-sensors"
+ - qcom,src-id:	Master port id
+ - qcom,dst-id:	Slave port id
+ - qcom,ab:	Arbitrated bandwidth in bytes/s
+ - qcom,ib:	Instantaneous bandwidth in bytes/s
+
+Example:
+
+	qcom,msm-adsp-sensors {
+		compatible = "qcom,msm-adsp-sensors";
+		qcom,src-id = <11>;
+		qcom,dst-id = <604>;
+		qcom,ab = <209715200>;
+		qcom,ib = <471859200>;
+	};
diff --git a/Documentation/devicetree/bindings/sound/qcom-audio-dev.txt b/Documentation/devicetree/bindings/sound/qcom-audio-dev.txt
index c47d442..104d6a2 100644
--- a/Documentation/devicetree/bindings/sound/qcom-audio-dev.txt
+++ b/Documentation/devicetree/bindings/sound/qcom-audio-dev.txt
@@ -87,6 +87,7 @@
                             BT SCO port ID value from 12288 to 12289
                             RT Proxy port ID values from 224 to 225 and 240 to 241
                             FM Rx and TX port ID values from 12292 to 12293
+                            incall record Rx and TX port ID values from 32771 to 32772
 
 * msm-auxpcm
 
@@ -286,6 +287,16 @@
 			compatible = "qcom,msm-dai-q6-dev";
 			qcom,msm-dai-q6-dev-id = <240>;
 		};
+
+		qcom,msm-dai-q6-incall-record-rx {
+			compatible = "qcom,msm-dai-q6-dev";
+			qcom,msm-dai-q6-dev-id = <32771>;
+		};
+
+		qcom,msm-dai-q6-incall-record-tx {
+			compatible = "qcom,msm-dai-q6-dev";
+			qcom,msm-dai-q6-dev-id = <32772>;
+		};
 	};
 
         qcom,msm-auxpcm {
@@ -347,6 +358,9 @@
 - qcom,taiko-mclk-clk-freq : Taiko mclk Freq in Hz. currently only 9600000Hz
 				is supported.
 
+Optional properties:
+- qcom,hdmi-audio-rx: specifies if HDMI audio support is enabled or not.
+
 Example:
 
 sound {
@@ -385,6 +399,8 @@
 	qcom,cdc-mclk-gpios = <&pm8941_gpios 15 0>;
 	taiko-mclk-clk = <&pm8941_clkdiv1>;
 	qcom,taiko-mclk-clk-freq = <9600000>;
+
+	qcom,hdmi-audio-rx;
 };
 
 * msm-dai-mi2s
diff --git a/Documentation/devicetree/bindings/sound/taiko_codec.txt b/Documentation/devicetree/bindings/sound/taiko_codec.txt
index 090d8db..74c25a0 100644
--- a/Documentation/devicetree/bindings/sound/taiko_codec.txt
+++ b/Documentation/devicetree/bindings/sound/taiko_codec.txt
@@ -34,7 +34,7 @@
  - qcom,cdc-micbias2-ext-cap: Boolean. Enable micbias 2 external capacitor mode.
  - qcom,cdc-micbias3-ext-cap: Boolean. Enable micbias 3 external capacitor mode.
  - qcom,cdc-micbias4-ext-cap: Boolean. Enable micbias 4 external capacitor mode.
-
+ - qcom,cdc-mclk-clk-rate - Specifies the master clock rate in Hz required for codec.
  - qcom,cdc-slim-ifd-dev - namme of the codec slim interface device.
  - qcom,cdc-slim-ifd-elemental-addr - codec slimbus slave interface device
 				     enumeration address.
@@ -86,7 +86,7 @@
 	qcom,cdc-micbias2-ext-cap;
 	qcom,cdc-micbias3-ext-cap;
 	qcom,cdc-micbias4-ext-cap;
-
+	qcom,cdc-mclk-clk-rate = <9600000>;
 	qcom,cdc-slim-ifd = "taiko-slim-ifd";
 	qcom,cdc-slim-ifd-elemental-addr = [00 00 A0 00 17 02];
 };
@@ -123,6 +123,7 @@
  - qcom,cdc-micbias2-ext-cap: Boolean. Enable micbias 2 external capacitor mode.
  - qcom,cdc-micbias3-ext-cap: Boolean. Enable micbias 3 external capacitor mode.
  - qcom,cdc-micbias4-ext-cap: Boolean. Enable micbias 4 external capacitor mode.
+ - qcom,cdc-mclk-clk-rate - Specifies the master clock rate in Hz required for codec.
 
 Example:
 i2c@f9925000 {
@@ -180,6 +181,7 @@
 		qcom,cdc-micbias2-cfilt-sel = <0x1>;
 		qcom,cdc-micbias3-cfilt-sel = <0x2>;
 		qcom,cdc-micbias4-cfilt-sel = <0x2>;
+		qcom,cdc-mclk-clk-rate = <12288000>;
 	};
 
 	wcd9xxx_codec@77{
diff --git a/Documentation/devicetree/bindings/thermal/qpnp-adc-tm.txt b/Documentation/devicetree/bindings/thermal/qpnp-adc-tm.txt
new file mode 100644
index 0000000..f1f4e94
--- /dev/null
+++ b/Documentation/devicetree/bindings/thermal/qpnp-adc-tm.txt
@@ -0,0 +1,120 @@
+Qualcomm's QPNP PMIC thermal monitor ADC driver (VADC_TM)
+
+QPNP PMIC thermal monitoring (TM) provides interface to thermal clients
+to set temperature thresholds and receive notification when the thresholds
+are crossed. A 15 bit ADC is used for measurements. The driver is part
+of the sysfs thermal framework that provides support to read the trip
+points, set threshold for the trip points and enable the trip points.
+Seperate kernel api's are provided to usb_id and batt_therm
+to set thresholds and receive threshold notifications.
+
+VADC_TM node
+
+Required properties:
+- compatible : should be "qcom,qpnp-adc-tm" for thermal ADC driver.
+- reg : offset and length of the PMIC Aribter register map.
+- address-cells : Must be one.
+- size-cells : Must be zero.
+- interrupts : The thermal ADC bank peripheral interrupts for eoc, high and low interrupts.
+- interrupt-names : Should be "eoc-int-en-set", "high-thr-en-set" and "low-thr-en-set".
+- qcom,adc-bit-resolution : Bit resolution of the ADC.
+- qcom,adc-vdd-reference : Voltage reference used by the ADC.
+
+Channel nodes
+NOTE: Atleast one Channel node is required.
+
+Required properties:
+- label : Channel name used for sysfs entry.
+- reg : AMUX channel number.
+- qcom,decimation : Sampling rate to use for the individual channel measurement.
+		    Select from the following unsigned int.
+		    0 : 512
+		    1 : 1K
+		    2 : 2K
+		    3 : 4K
+- qcom,pre-div-channel-scaling : Pre-div used for the channel before the signal is being measured.
+				 Select from the following unsigned int for the corresponding
+				 numerator/denominator pre-div ratio.
+				 0 : pre-div ratio of {1, 1}
+				 1 : pre-div ratio of {1, 3}
+				 2 : pre-div ratio of {1, 4}
+				 3 : pre-div ratio of {1, 6}
+				 4 : pre-div ratio of {1, 20}
+- qcom,calibration-type : Reference voltage to use for channel calibration.
+			  Channel calibration is dependendent on the channel.
+			  Certain channels like XO_THERM, BATT_THERM use ratiometric
+			  calibration. Most other channels fall under absolute calibration.
+			  Select from the following strings.
+			  "absolute" : Uses the 625mv and 1.25V reference channels.
+			  "ratiometric" : Uses the reference Voltage/GND for calibration.
+- qcom,scale-function : Scaling fuction used to convert raw ADC code to units specific to
+			a given channel.
+			Select from the following unsigned int.
+			0 : Default scaling to convert raw adc code to voltage.
+			1 : Conversion to temperature based on btm parameters.
+			2 : Returns result in milli degree's Centigrade.
+			3 : Returns current across 0.1 ohm resistor.
+			4 : Returns XO thermistor voltage in degree's Centigrade.
+- qcom,hw-settle-time : Settling period for the channel before ADC read.
+			Select from the following unsigned int.
+			0 : 0us
+			1 : 100us
+			2 : 200us
+			3 : 300us
+			4 : 400us
+			5 : 500us
+			6 : 600us
+			7 : 700us
+			8 : 800us
+			9 : 900us
+			0xa : 1ms
+			0xb : 2ms
+			0xc : 4ms
+			0xd : 6ms
+			0xe : 8ms
+			0xf : 10ms
+- qcom,fast-avg-setup : Average number of samples to be used for measurement. Fast averaging
+			provides the option to obtain a single measurement from the ADC that
+			is an average of multiple samples. The value selected is 2^(value)
+			Select from
+			0 : 1
+			1 : 2
+			2 : 4
+			3 : 8
+			4 : 16
+			5 : 32
+			6 : 64
+			7 : 128
+			8 : 256
+- qcom,btm-channel-number : There are 5 BTM channels. The BTM channel numbers are statically
+			    allocated to the corresponding channel node.
+
+Example:
+	/* Main Node */
+	qcom,vadc@3400 {
+                        compatible = "qcom,qpnp-adc-tm";
+                        reg = <0x3400 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+                        interrupts = <0x0 0x34 0x0>,
+					<0x0 0x34 0x3>,
+					<0x0 0x34 0x4>;
+			interrupt-names = "eoc-int-en-set",
+					  "high-thr-en-set",
+					  "low-thr-en-set";
+                        qcom,adc-bit-resolution = <15>;
+                        qcom,adc-vdd-reference = <1800>;
+
+			/* Channel Node */
+                        chan@b5 {
+                                label = "pa_therm1";
+				reg = <0xb5>;
+                                qcom,decimation = <0>;
+                                qcom,pre-div-channel-scaling = <0>;
+                                qcom,calibration-type = "absolute";
+                                qcom,scale-function = <2>;
+                                qcom,hw-settle-time = <0>;
+                                qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x70>;
+                        };
+	};
diff --git a/Documentation/devicetree/bindings/usb/msm-ehci-hsic.txt b/Documentation/devicetree/bindings/usb/msm-ehci-hsic.txt
index 0e59f69..ffb0c6a 100644
--- a/Documentation/devicetree/bindings/usb/msm-ehci-hsic.txt
+++ b/Documentation/devicetree/bindings/usb/msm-ehci-hsic.txt
@@ -9,6 +9,17 @@
 - <supply-name>-supply: handle to the regulator device tree node
   Required "supply-name" is "HSIC_VDDCX" and optionally - "HSIC_GDSC".
 
+Optional properties :
+- hsic,<gpio-name>-gpio : handle to the GPIO node, see "gpios property"
+  in Documentation/devicetree/bindings/gpio/gpio.txt.
+  Optional "gpio-name" can be "strobe" and "data".
+- hsic,ignore-cal-pad-config : If present then HSIC CAL PAD configuration
+  using TLMM is not performed.
+- hsic,strobe-pad-offset : Offset of TLMM register for configuring HSIC
+  STROBE GPIO PAD.
+- hsic,data-pad-offset : Offset of TLMM register for configuring HSIC
+  DATA GPIO PAD.
+
 Example MSM HSIC EHCI controller device node :
 	hsic@f9a15000 {
 		compatible = "qcom,hsic-host";
@@ -17,4 +28,47 @@
 		interrupt-names = "core_irq";
 		HSIC_VDDCX-supply = <&pm8019_l12>;
 		HSIC_GDSC-supply = <&gdsc_usb_hsic>;
+		hsic,strobe-gpio = <&msmgpio 144 0x00>;
+		hsic,data-gpio = <&msmgpio 145 0x00>;
+		hsic,ignore-cal-pad-config;
+		hsic,strobe-pad-offset = <0x2050>;
+		hsic,data-pad-offset = <0x2054>;
 	};
+
+SMSC HSIC HUB
+
+Required properties :
+- compatible : should be "qcom,hsic-smsc-hub"
+- smsc,<gpio-name>-gpio : handle to the GPIO node, see "gpios property"
+  in Documentation/devicetree/bindings/gpio/gpio.txt.
+  Required "gpio-name" is "reset" and optionally - "refclk", "int".
+- <supply-name>-supply: handle to the regulator device tree node
+  Required "supply-name" is "hub_init" and optionally - "hub_vbus".
+- Sub node for "MSM HSIC EHCI controller".
+  Sub node has the required properties mentioned above.
+
+Example SMSC HSIC HUB :
+	hsic_hub {
+		compatible = "qcom,hsic-smsc-hub";
+		ranges;
+		smsc,reset-gpio = <&pm8941_gpios 8 0x00>;
+		smsc,refclk-gpio = <&pm8941_gpios 16 0x00>;
+		smsc,int-gpio = <&msmgpio 50 0x00>;
+		hub_int-supply = <&pm8941_l10>;
+		hub_vbus-supply = <&pm8941_mvs1>;
+
+		hsic@f9a00000 {
+			compatible = "qcom,hsic-host";
+			reg = <0xf9a00000 0x400>;
+			interrupts = <0 136 0>;
+			interrupt-names = "core_irq";
+			HSIC_VDDCX-supply = <&pm8841_s2>;
+			HSIC_GDSC-supply = <&gdsc_usb_hsic>;
+			hsic,strobe-gpio = <&msmgpio 144 0x00>;
+			hsic,data-gpio = <&msmgpio 145 0x00>;
+			hsic,ignore-cal-pad-config;
+			hsic,strobe-pad-offset = <0x2050>;
+			hsic,data-pad-offset = <0x2054>;
+		};
+	};
+
diff --git a/Documentation/mmc/mmc-dev-attrs.txt b/Documentation/mmc/mmc-dev-attrs.txt
index 86d38ca..43453d0 100644
--- a/Documentation/mmc/mmc-dev-attrs.txt
+++ b/Documentation/mmc/mmc-dev-attrs.txt
@@ -39,7 +39,7 @@
 
 All attributes are read-only.
 
-	cid			Card Identifaction Register
+	cid			Card Identification Register
 	csd			Card Specific Data Register
 	scr			SD Card Configuration Register (SD only)
 	date			Manufacturing Date (from CID Register)
@@ -107,3 +107,41 @@
 	clkgate_delay	Tune the clock gating delay with desired value in milliseconds.
 
 echo <desired delay> > /sys/class/mmc_host/mmcX/clkgate_delay
+
+SD/MMC/SDIO Clock Scaling Attributes
+====================================
+
+Read and write accesses are provided to following attributes.
+
+	polling_interval	Measured in milliseconds, this attribute
+				defines how often we need to check the card
+				usage and make decisions on frequency scaling.
+
+	up_threshold		This attribute defines what should be the
+				average card usage between the polling
+				interval for the mmc core to make a decision
+				on whether it should increase the frequency.
+				For example when it is set to '35' it means
+				that between the checking intervals the card
+				needs to be on average more than 35% in use to
+				scale up the frequency. The value should be
+				between 0 - 100 so that it can be compared
+				against load percentage.
+
+	down_threshold		Similar to up_threshold, but on lowering the
+				frequency. For example, when it is set to '2'
+				it means that between the checking intervals
+				the card needs to be on average less than 2%
+				in use to scale down the clocks to minimum
+				frequency. The value should be between 0 - 100
+				so that it can be compared against load
+				percentage.
+
+	enable			Enable clock scaling for hosts (and cards)
+				that support ultrahigh speed modes
+				(SDR104, DDR50, HS200).
+
+echo <desired value> > /sys/class/mmc_host/mmcX/clk_scaling/polling_interval
+echo <desired value> > /sys/class/mmc_host/mmcX/clk_scaling/up_threshold
+echo <desired value> > /sys/class/mmc_host/mmcX/clk_scaling/down_threshold
+echo <desired value> > /sys/class/mmc_host/mmcX/clk_scaling/enable
diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 89c7417..53fd3b2 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -1889,7 +1889,6 @@
 
 config DONT_MAP_HOLE_AFTER_MEMBANK0
 	def_bool n
-	depends on SPARSEMEM
 
 config ARCH_ENABLE_MEMORY_HOTPLUG
 	def_bool n
diff --git a/arch/arm/boot/compressed/head.S b/arch/arm/boot/compressed/head.S
index ace2dee..64a6d6f 100644
--- a/arch/arm/boot/compressed/head.S
+++ b/arch/arm/boot/compressed/head.S
@@ -567,6 +567,12 @@
 		mcr	p15, 0, r0, c7, c0, 0	@ invalidate whole cache v3
 		mov	pc, lr
 
+#ifdef CONFIG_CPU_DCACHE_WRITETHROUGH
+#define CB_BITS 0x08
+#else
+#define CB_BITS 0x0c
+#endif
+
 __setup_mmu:	sub	r3, r4, #16384		@ Page directory size
 		bic	r3, r3, #0xff		@ Align the pointer
 		bic	r3, r3, #0x3f00
@@ -578,17 +584,14 @@
 		mov	r9, r0, lsr #18
 		mov	r9, r9, lsl #18		@ start of RAM
 		add	r10, r9, #0x10000000	@ a reasonable RAM size
-		mov	r1, #0x12
-		orr	r1, r1, #3 << 10
+		mov	r1, #0x12		@ XN|U + section mapping
+		orr	r1, r1, #3 << 10	@ AP=11
 		add	r2, r3, #16384
 1:		cmp	r1, r9			@ if virt > start of RAM
-#ifdef CONFIG_CPU_DCACHE_WRITETHROUGH
-		orrhs	r1, r1, #0x08		@ set cacheable
-#else
-		orrhs	r1, r1, #0x0c		@ set cacheable, bufferable
-#endif
-		cmp	r1, r10			@ if virt > end of RAM
-		bichs	r1, r1, #0x0c		@ clear cacheable, bufferable
+		cmphs	r10, r1			@   && end of RAM > virt
+		bic	r1, r1, #0x1c		@ clear XN|U + C + B
+		orrlo	r1, r1, #0x10		@ Set XN|U for non-RAM
+		orrhs	r1, r1, r6		@ set RAM section settings
 		str	r1, [r0], #4		@ 1:1 mapping
 		add	r1, r1, #1048576
 		teq	r0, r2
@@ -599,7 +602,7 @@
  * so there is no map overlap problem for up to 1 MB compressed kernel.
  * If the execution is in RAM then we would only be duplicating the above.
  */
-		mov	r1, #0x1e
+		orr	r1, r6, #0x04		@ ensure B is set for this
 		orr	r1, r1, #3 << 10
 		mov	r2, pc
 		mov	r2, r2, lsr #20
@@ -620,6 +623,7 @@
 __armv4_mmu_cache_on:
 		mov	r12, lr
 #ifdef CONFIG_MMU
+		mov	r6, #CB_BITS | 0x12	@ U
 		bl	__setup_mmu
 		mov	r0, #0
 		mcr	p15, 0, r0, c7, c10, 4	@ drain write buffer
@@ -641,6 +645,7 @@
 #ifdef CONFIG_MMU
 		mrc	p15, 0, r11, c0, c1, 4	@ read ID_MMFR0
 		tst	r11, #0xf		@ VMSA
+		movne	r6, #CB_BITS | 0x02	@ !XN
 		blne	__setup_mmu
 		mov	r0, #0
 		mcr	p15, 0, r0, c7, c10, 4	@ drain write buffer
@@ -655,7 +660,7 @@
 		orr	r0, r0, #1 << 25	@ big-endian page tables
 #endif
 		orrne	r0, r0, #1		@ MMU enabled
-		movne	r1, #-1
+		movne	r1, #0xfffffffd		@ domain 0 = client
 		mcrne	p15, 0, r3, c2, c0, 0	@ load page table pointer
 		mcrne	p15, 0, r1, c3, c0, 0	@ load domain access control
 #endif
@@ -668,6 +673,7 @@
 
 __fa526_cache_on:
 		mov	r12, lr
+		mov	r6, #CB_BITS | 0x12	@ U
 		bl	__setup_mmu
 		mov	r0, #0
 		mcr	p15, 0, r0, c7, c7, 0	@ Invalidate whole cache
@@ -682,6 +688,7 @@
 
 __arm6_mmu_cache_on:
 		mov	r12, lr
+		mov	r6, #CB_BITS | 0x12	@ U
 		bl	__setup_mmu
 		mov	r0, #0
 		mcr	p15, 0, r0, c7, c0, 0	@ invalidate whole cache v3
diff --git a/arch/arm/boot/dts/mpq8092.dtsi b/arch/arm/boot/dts/mpq8092.dtsi
index 0decddc..0ab0a0c 100644
--- a/arch/arm/boot/dts/mpq8092.dtsi
+++ b/arch/arm/boot/dts/mpq8092.dtsi
@@ -35,6 +35,7 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		interrupts = <0 208 0>;
 	};
 
 	timer {
@@ -270,6 +271,12 @@
 		qcom,bus-speed-mode = "SDR12", "SDR25", "SDR50", "DDR50", "SDR104";
 		qcom,current-limit = <800>;
 	};
+
+	sata: sata@fc580000 {
+		compatible = "qcom,msm-ahci";
+		reg = <0xfc580000 0x17c>;
+		interrupts = <0 243 0>;
+	};
 };
 
 &gdsc_venus {
diff --git a/arch/arm/boot/dts/msm-iommu-v1.dtsi b/arch/arm/boot/dts/msm-iommu-v1.dtsi
new file mode 100644
index 0000000..5a08f51
--- /dev/null
+++ b/arch/arm/boot/dts/msm-iommu-v1.dtsi
@@ -0,0 +1,228 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/ {
+	lpass_iommu: qcom,iommu@fd000000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd000000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "lpass_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd000000 {
+			reg = <0xfd000000 0x1000>;
+			interrupts = <0 250 0>;
+			qcom,iommu-ctx-mids = <0 15>;
+			label = "q6_fw";
+		};
+
+		qcom,iommu-ctx@fd001000 {
+			reg = <0xfd001000 0x1000>;
+			interrupts = <0 250 0>;
+			qcom,iommu-ctx-mids = <1>;
+			label = "audio_shared";
+		};
+
+		qcom,iommu-ctx@fd002000 {
+			reg = <0xfd002000 0x1000>;
+			interrupts = <0 250 0>;
+			qcom,iommu-ctx-mids = <2>;
+			label = "video_shared";
+		};
+
+		qcom,iommu-ctx@fd003000 {
+			reg = <0xfd003000 0x1000>;
+			interrupts = <0 250 0>;
+			qcom,iommu-ctx-mids = <3 4 5 6 7 8 9 10 11 12 13 14>;
+			label = "q6_spare";
+		};
+	};
+
+	copss_iommu: qcom,iommu@fd010000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd010000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "copss_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd010000 {
+			reg = <0xfd010000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <0>;
+			label = "copss_0";
+		};
+
+		qcom,iommu-ctx@fd011000 {
+			reg = <0xfd011000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <1>;
+			label = "copss_1";
+		};
+
+		qcom,iommu-ctx@fd012000 {
+			reg = <0xfd012000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <2>;
+			label = "copss_2";
+		};
+
+		qcom,iommu-ctx@fd013000 {
+			reg = <0xfd013000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <3>;
+			label = "copss_3";
+		};
+
+		qcom,iommu-ctx@fd014000 {
+			reg = <0xfd014000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <4>;
+			label = "copss_4";
+		};
+
+		qcom,iommu-ctx@fd015000 {
+			reg = <0xfd015000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <5>;
+			label = "copss_5";
+		};
+
+		qcom,iommu-ctx@fd016000 {
+			reg = <0xfd016000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <6>;
+			label = "copss_6";
+		};
+
+		qcom,iommu-ctx@fd017000 {
+			reg = <0xfd017000 0x1000>;
+			interrupts = <0 254 0>;
+			qcom,iommu-ctx-mids = <7>;
+			label = "copss_7";
+		};
+	};
+
+	mdpe_iommu: qcom,iommu@fd860000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd860000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "mdpe_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd860000 {
+			reg = <0xfd860000 0x1000>;
+			interrupts = <0 247 0>;
+			qcom,iommu-ctx-mids = <>;
+			label = "mdpe_0";
+		};
+
+		qcom,iommu-ctx@fd861000 {
+			reg = <0xfd861000 0x1000>;
+			interrupts = <0 247 0>;
+			qcom,iommu-ctx-mids = <>;
+			label = "mdpe_1";
+		};
+	};
+
+	mdps_iommu: qcom,iommu@fd870000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd870000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "mdps_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd870000 {
+			reg = <0xfd870000 0x1000>;
+			interrupts = <0 247 0>;
+			qcom,iommu-ctx-mids = <>;
+			label = "mdps_0";
+		};
+
+		qcom,iommu-ctx@fd871000 {
+			reg = <0xfd871000 0x1000>;
+			interrupts = <0 247 0>;
+			qcom,iommu-ctx-mids = <>;
+			label = "mdps_1";
+		};
+	};
+
+	gfx_iommu: qcom,iommu@fd880000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd880000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "gfx_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd880000 {
+			reg = <0xfd880000 0x1000>;
+			interrupts = <0 241 0>;
+			qcom,iommu-ctx-mids = <0 1 2 3 4 5 6 7 8 9 10 11 12 13
+					       14 15>;
+			label = "gfx3d_user";
+		};
+
+		qcom,iommu-ctx@fd881000 {
+			reg = <0xfd881000 0x1000>;
+			interrupts = <0 241 0>;
+			qcom,iommu-ctx-mids = <16 17 18 19 20 21 22 23 24 25
+					       26 27 28 29 30 31>;
+			label = "gfx3d_priv";
+		};
+
+		qcom,iommu-ctx@fd882000 {
+			reg = <0xfd882000 0x1000>;
+			interrupts = <0 241 0>;
+			qcom,iommu-ctx-mids = <>;
+			label = "gfx3d_spare";
+		};
+	};
+
+	vfe_iommu: qcom,iommu@fd890000 {
+		compatible = "qcom,msm-smmu-v1";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		reg = <0xfd890000 0x10000>;
+		qcom,glb-offset = <0xF000>;
+		label = "vfe_iommu";
+		status = "disabled";
+
+		qcom,iommu-ctx@fd890000 {
+			reg = <0xfd890000 0x1000>;
+			interrupts = <0 65 0>;
+			qcom,iommu-ctx-mids = <0>;
+			label = "vfe0";
+		};
+
+		qcom,iommu-ctx@fd891000 {
+			reg = <0xfd891000 0x1000>;
+			interrupts = <0 65 0>;
+			qcom,iommu-ctx-mids = <1>;
+			label = "vfe1";
+		};
+	};
+};
diff --git a/arch/arm/boot/dts/msm-pm8019.dtsi b/arch/arm/boot/dts/msm-pm8019.dtsi
index 2105e8a..322e601 100755
--- a/arch/arm/boot/dts/msm-pm8019.dtsi
+++ b/arch/arm/boot/dts/msm-pm8019.dtsi
@@ -156,13 +156,15 @@
 		pm8019_vadc: vadc@3100 {
 			compatible = "qcom,qpnp-vadc";
 			reg = <0x3100 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			interrupts = <0x0 0x31 0x0>;
 			qcom,adc-bit-resolution = <15>;
 			qcom,adc-vdd-reference = <1800>;
 
 			chan@8 {
 				label = "die_temp";
-				qcom,channel-num = <8>;
+				reg = <8>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
@@ -173,7 +175,7 @@
 
 			chan@9 {
 				label = "ref_625mv";
-				qcom,channel-num = <9>;
+				reg = <9>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
@@ -182,9 +184,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@10 {
+			chan@a {
 				label = "ref_1250v";
-				qcom,channel-num = <10>;
+				reg = <0xa>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
diff --git a/arch/arm/boot/dts/msm-pm8026.dtsi b/arch/arm/boot/dts/msm-pm8026.dtsi
new file mode 100644
index 0000000..94db3ea
--- /dev/null
+++ b/arch/arm/boot/dts/msm-pm8026.dtsi
@@ -0,0 +1,18 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+&spmi_bus {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	interrupt-controller;
+	#interrupt-cells = <3>;
+};
diff --git a/arch/arm/boot/dts/msm-pm8110.dtsi b/arch/arm/boot/dts/msm-pm8110.dtsi
new file mode 100644
index 0000000..94db3ea
--- /dev/null
+++ b/arch/arm/boot/dts/msm-pm8110.dtsi
@@ -0,0 +1,18 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+&spmi_bus {
+	#address-cells = <1>;
+	#size-cells = <0>;
+	interrupt-controller;
+	#interrupt-cells = <3>;
+};
diff --git a/arch/arm/boot/dts/msm-pm8941.dtsi b/arch/arm/boot/dts/msm-pm8941.dtsi
index 6538db5..098f543 100644
--- a/arch/arm/boot/dts/msm-pm8941.dtsi
+++ b/arch/arm/boot/dts/msm-pm8941.dtsi
@@ -58,32 +58,14 @@
 			};
 		};
 
-		pm8941_bms: bms@4000 {
+		pm8941_bms: qcom,bms {
+			spmi-dev-container;
+			compatible = "qcom,qpnp-bms";
 			#address-cells = <1>;
 			#size-cells = <1>;
 			status = "disabled";
-			compatible = "qcom,qpnp-bms";
-			reg = <0x4000 0x100>;
 
-			interrupts =	<0x0 0x40 0x0>,
-					<0x0 0x40 0x1>,
-					<0x0 0x40 0x2>,
-					<0x0 0x40 0x3>,
-					<0x0 0x40 0x4>,
-					<0x0 0x40 0x5>,
-					<0x0 0x40 0x6>,
-					<0x0 0x40 0x7>;
-
-			interrupt-names = "vsense_for_r",
-					  "vsense_avg",
-					  "sw_cc_thr",
-					  "ocv_thr",
-					  "charge_begin",
-					  "good_ocv",
-					  "ocv_for_r",
-					  "cc_thr";
-
-			qcom,bms-r-sense-mohm = <10>;
+			qcom,bms-r-sense-mohm = <2>;
 			qcom,bms-v-cutoff-uv = <3400000>;
 			qcom,bms-max-voltage-uv = <4200000>;
 			qcom,bms-r-conn-mohm = <18>;
@@ -95,7 +77,32 @@
 			qcom,bms-calculate-soc-ms = <20000>;
 			qcom,bms-chg-term-ua = <100000>;
 			qcom,bms-batt-type = <0>;
-			qcom,bms-use-voltage-soc;
+			qcom,bms-use-external-rsense;
+
+			qcom,bms-iadc@3800 {
+				reg = <0x3800 0x100>;
+			};
+
+			qcom,bms-bms@4000 {
+				reg = <0x4000 0x100>;
+				interrupts =	<0x0 0x40 0x0>,
+						<0x0 0x40 0x1>,
+						<0x0 0x40 0x2>,
+						<0x0 0x40 0x3>,
+						<0x0 0x40 0x4>,
+						<0x0 0x40 0x5>,
+						<0x0 0x40 0x6>,
+						<0x0 0x40 0x7>;
+
+				interrupt-names = "vsense_for_r",
+						  "vsense_avg",
+						  "sw_cc_thr",
+						  "ocv_thr",
+						  "charge_begin",
+						  "good_ocv",
+						  "ocv_for_r",
+						  "cc_thr";
+			};
 		};
 
 		clkdiv@5b00 {
@@ -126,8 +133,10 @@
 			qcom,chg-vddmax-mv = <4200>;
 			qcom,chg-vddsafe-mv = <4200>;
 			qcom,chg-vinmin-mv = <4200>;
+			qcom,chg-vbatdet-mv = <4100>;
 			qcom,chg-ibatmax-ma = <1500>;
 			qcom,chg-ibatterm-ma = <200>;
+			qcom,chg-ibatsafe-ma = <1500>;
 
 			qcom,chg-chgr@1000 {
 				status = "disabled";
@@ -486,6 +495,8 @@
 		vadc@3100 {
 			compatible = "qcom,qpnp-vadc";
 			reg = <0x3100 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			interrupts = <0x0 0x31 0x0>;
 			interrupt-names = "eoc-int-en-set";
 			qcom,adc-bit-resolution = <15>;
@@ -493,7 +504,7 @@
 
 			chan@0 {
 				label = "usb_in";
-				qcom,channel-num = <0>;
+				reg = <0>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <4>;
 				qcom,calibration-type = "absolute";
@@ -504,7 +515,7 @@
 
 			chan@1 {
 				label = "dc_in";
-				qcom,channel-num = <1>;
+				reg = <1>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <4>;
 				qcom,calibration-type = "absolute";
@@ -515,7 +526,7 @@
 
 			chan@2 {
 				label = "vchg_sns";
-				qcom,channel-num = <2>;
+				reg = <2>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <3>;
 				qcom,calibration-type = "absolute";
@@ -526,7 +537,7 @@
 
 			chan@3 {
 				label = "spare1";
-				qcom,channel-num = <3>;
+				reg = <3>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <6>;
 				qcom,calibration-type = "absolute";
@@ -537,7 +548,7 @@
 
 			chan@4 {
 				label = "spare2";
-				qcom,channel-num = <4>;
+				reg = <4>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <6>;
 				qcom,calibration-type = "absolute";
@@ -548,7 +559,7 @@
 
 			chan@5 {
 				label = "vcoin";
-				qcom,channel-num = <5>;
+				reg = <5>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <1>;
 				qcom,calibration-type = "absolute";
@@ -559,7 +570,7 @@
 
 			chan@6 {
 				label = "vbat_sns";
-				qcom,channel-num = <6>;
+				reg = <6>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <1>;
 				qcom,calibration-type = "absolute";
@@ -570,7 +581,7 @@
 
 			chan@7 {
 				label = "vph_pwr";
-				qcom,channel-num = <7>;
+				reg = <7>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <1>;
 				qcom,calibration-type = "absolute";
@@ -581,7 +592,7 @@
 
 			chan@8 {
 				label = "die_temp";
-				qcom,channel-num = <8>;
+				reg = <8>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
@@ -592,7 +603,7 @@
 
 			chan@9 {
 				label = "ref_625mv";
-				qcom,channel-num = <9>;
+				reg = <9>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
@@ -601,9 +612,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@10 {
+			chan@a {
 				label = "ref_1250v";
-				qcom,channel-num = <10>;
+				reg = <0xa>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "absolute";
@@ -612,9 +623,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@48 {
+			chan@30 {
 				label = "batt_therm";
-				qcom,channel-num = <48>;
+				reg = <0x30>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -623,9 +634,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@49 {
+			chan@31 {
 				label = "batt_id";
-				qcom,channel-num = <49>;
+				reg = <0x31>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -634,9 +645,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@178 {
+			chan@b2 {
 				label = "xo_therm_pu2";
-				qcom,channel-num = <178>;
+				reg = <0xb2>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -645,9 +656,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@179 {
+			chan@b3 {
 				label = "msm_therm";
-				qcom,channel-num = <179>;
+				reg = <0xb3>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -656,9 +667,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@180 {
+			chan@b4 {
 				label = "emmc_therm";
-				qcom,channel-num = <180>;
+				reg = <0xb4>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -667,9 +678,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@181 {
+			chan@b5 {
 				label = "pa_therm1";
-				qcom,channel-num = <181>;
+				reg = <0xb5>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -678,9 +689,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@183 {
+			chan@b7 {
 				label = "pa_therm2";
-				qcom,channel-num = <183>;
+				reg = <0xb7>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -689,9 +700,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@184 {
+			chan@b8 {
 				label = "quiet_therm";
-				qcom,channel-num = <184>;
+				reg = <0xb8>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -700,9 +711,9 @@
 				qcom,fast-avg-setup = <0>;
 			};
 
-			chan@185 {
+			chan@b9 {
 				label = "usb_id";
-				qcom,channel-num = <185>;
+				reg = <0xb9>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <0>;
 				qcom,calibration-type = "ratiometric";
@@ -715,6 +726,8 @@
 		iadc@3600 {
 			compatible = "qcom,qpnp-iadc";
 			reg = <0x3600 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
 			interrupts = <0x0 0x36 0x0>;
 			interrupt-names = "eoc-int-en-set";
 			qcom,adc-bit-resolution = <16>;
@@ -723,7 +736,7 @@
 
 			chan@0 {
 				label = "internal_rsense";
-				qcom,channel-num = <0>;
+				reg = <0>;
 				qcom,decimation = <0>;
 				qcom,pre-div-channel-scaling = <1>;
 				qcom,calibration-type = "absolute";
@@ -732,6 +745,82 @@
 				qcom,fast-avg-setup = <0>;
 			};
 		};
+
+		qcom,vadc@3400 {
+			compatible = "qcom,qpnp-adc-tm";
+			reg = <0x3400 0x100>;
+			#address-cells = <1>;
+			#size-cells = <0>;
+			interrupts =	<0x0 0x34 0x0>,
+					<0x0 0x34 0x3>,
+				     <0x0 0x34 0x4>;
+			interrupt-names =	"eoc-int-en-set",
+						"high-thr-en-set",
+						"low-thr-en-set";
+			qcom,adc-bit-resolution = <15>;
+			qcom,adc-vdd-reference = <1800>;
+
+			/* Channel Node */
+			chan@b9 {
+				label = "usb_id";
+				reg = <0xb9>;
+				qcom,decimation = <0>;
+				qcom,pre-div-channel-scaling = <0>;
+				qcom,calibration-type = "ratiometric";
+				qcom,scale-function = <2>;
+				qcom,hw-settle-time = <0>;
+				qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x48>;
+			};
+
+			chan@30 {
+				label = "batt_therm";
+				reg = <0x30>;
+				qcom,decimation = <0>;
+				qcom,pre-div-channel-scaling = <0>;
+				qcom,calibration-type = "ratiometric";
+				qcom,scale-function = <1>;
+				qcom,hw-settle-time = <0xf>;
+				qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x68>;
+			};
+
+			chan@b5 {
+				label = "pa_therm1";
+				reg = <0xb5>;
+				qcom,decimation = <0>;
+				qcom,pre-div-channel-scaling = <0>;
+				qcom,calibration-type = "absolute";
+				qcom,scale-function = <2>;
+				qcom,hw-settle-time = <0>;
+				qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x70>;
+			};
+
+			chan@b7 {
+				label = "pa_therm2";
+				reg = <0xb7>;
+				qcom,decimation = <0>;
+				qcom,pre-div-channel-scaling = <0>;
+				qcom,calibration-type = "ratiometric";
+				qcom,scale-function = <2>;
+				qcom,hw-settle-time = <0>;
+				qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x78>;
+			};
+
+			chan@b4 {
+				label = "emmc_therm";
+				reg = <0xb4>;
+				qcom,decimation = <0>;
+				qcom,pre-div-channel-scaling = <0>;
+				qcom,calibration-type = "ratiometric";
+				qcom,scale-function = <2>;
+				qcom,hw-settle-time = <0>;
+				qcom,fast-avg-setup = <0>;
+				qcom,btm-channel-number = <0x80>;
+			};
+		};
 	};
 
 	qcom,pm8941@1 {
diff --git a/arch/arm/boot/dts/msm8226-sim.dts b/arch/arm/boot/dts/msm8226-sim.dts
index 41ac69d..9a0ec17 100644
--- a/arch/arm/boot/dts/msm8226-sim.dts
+++ b/arch/arm/boot/dts/msm8226-sim.dts
@@ -23,3 +23,54 @@
 		status = "ok";
 	};
 };
+
+&sdcc1 {
+	qcom,vdd-always-on;
+	qcom,vdd-lpm-sup;
+	qcom,vdd-voltage-level = <2950000 2950000>;
+	qcom,vdd-current-level = <800 500000>;
+
+	qcom,vdd-io-always-on;
+	qcom,vdd-io-voltage-level = <1800000 1800000>;
+	qcom,vdd-io-current-level = <250 154000>;
+
+	qcom,pad-pull-on = <0x0 0x3 0x3>; /* no-pull, pull-up, pull-up */
+	qcom,pad-pull-off = <0x0 0x3 0x3>; /* no-pull, pull-up, pull-up */
+	qcom,pad-drv-on = <0x7 0x4 0x4>; /* 16mA, 10mA, 10mA */
+	qcom,pad-drv-off = <0x0 0x0 0x0>; /* 2mA, 2mA, 2mA */
+
+	vdd-supply = <&pm8026_l17>;
+	vdd-io-supply = <&pm8026_l6>;
+	qcom,clk-rates = <400000 25000000 50000000 100000000 200000000>;
+	qcom,sup-voltages = <2950 2950>;
+
+	qcom,bus-speed-mode = "HS200_1p8v", "DDR_1p8v";
+	qcom,nonremovable;
+	status = "ok";
+};
+
+&sdcc2 {
+	vdd-supply = <&pm8026_l18>;
+	vdd-io-supply = <&pm8026_l21>;
+	qcom,vdd-voltage-level = <2950000 2950000>;
+	qcom,vdd-current-level = <9000 800000>;
+
+	qcom,vdd-io-always-on;
+	qcom,vdd-io-lpm-sup;
+	qcom,vdd-io-voltage-level = <1800000 2950000>;
+	qcom,vdd-io-current-level = <6 22000>;
+
+	qcom,pad-pull-on = <0x0 0x3 0x3>; /* no-pull, pull-up, pull-up */
+	qcom,pad-pull-off = <0x0 0x3 0x3>; /* no-pull, pull-up, pull-up */
+	qcom,pad-drv-on = <0x7 0x4 0x4>; /* 16mA, 10mA, 10mA */
+	qcom,pad-drv-off = <0x0 0x0 0x0>; /* 2mA, 2mA, 2mA */
+
+	qcom,clk-rates = <400000 25000000 50000000 100000000 200000000>;
+	qcom,sup-voltages = <2950 2950>;
+
+	qcom,xpc;
+	qcom,bus-speed-mode = "SDR12", "SDR25", "SDR50", "DDR50", "SDR104";
+	qcom,current-limit = <800>;
+
+	status = "ok";
+};
diff --git a/arch/arm/boot/dts/msm8226.dtsi b/arch/arm/boot/dts/msm8226.dtsi
index 927ebcd..475ed40 100644
--- a/arch/arm/boot/dts/msm8226.dtsi
+++ b/arch/arm/boot/dts/msm8226.dtsi
@@ -32,7 +32,9 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		gpio-controller;
 		#gpio-cells = <2>;
+		interrupts = <0 208 0>;
 	};
 
 	timer {
@@ -55,6 +57,13 @@
 		status = "disabled";
 	};
 
+	qcom,sps@f9984000 {
+		compatible = "qcom,msm_sps";
+		reg = <0xf9984000 0x15000>,
+		      <0xf9999000 0xb000>;
+		interrupts = <0 94 0>;
+	};
+
         usb@f9a55000 {
 		compatible = "qcom,hsusb-otg";
 		reg = <0xf9a55000 0x400>;
@@ -151,6 +160,165 @@
 		};
 	};
 
+	sdcc1: qcom,sdcc@f9824000 {
+		cell-index = <1>; /* SDC1 eMMC slot */
+		compatible = "qcom,msm-sdcc";
+
+		reg = <0xf9824000 0x800>,
+			<0xf9824800 0x100>,
+			<0xf9804000 0x7000>;
+		reg-names = "core_mem", "dml_mem", "bam_mem";
+		interrupts = <0 123 0>, <0 137 0>;
+		interrupt-names = "core_irq", "bam_irq";
+
+		qcom,bus-width = <8>;
+		status = "disabled";
+	};
+
+	sdcc2: qcom,sdcc@f98a4000 {
+		cell-index = <2>; /* SDC2 SD card slot */
+		compatible = "qcom,msm-sdcc";
+
+		reg = <0xf98a4000 0x800>,
+			<0xf98a4800 0x100>,
+			<0xf9884000 0x7000>;
+		reg-names = "core_mem", "dml_mem", "bam_mem";
+		interrupts = <0 125 0>, <0 220 0>;
+		interrupt-names = "core_irq", "bam_irq";
+
+		qcom,bus-width = <4>;
+		status = "disabled";
+	};
+
+	spmi_bus: qcom,spmi@fc4c0000 {
+		cell-index = <0>;
+		compatible = "qcom,spmi-pmic-arb";
+		reg = <0xfc4cf000 0x1000>,
+		      <0Xfc4cb000 0x1000>;
+		/* 190,ee0_krait_hlos_spmi_periph_irq */
+		/* 187,channel_0_krait_hlos_trans_done_irq */
+		interrupts = <0 190 0>, <0 187 0>;
+		qcom,not-wakeup;
+		qcom,pmic-arb-ee = <0>;
+		qcom,pmic-arb-channel = <0>;
+		qcom,pmic-arb-ppid-map = <0x001000a0>, /* PM8026_0 */
+					 <0x005000a2>, /* INTERRUPT */
+					 <0x006000a3>, /* SPMI_0 */
+					 <0x00800000>, /* PON0 */
+					 <0x00a000a5>, /* VREF_LPDDR3 */
+					 <0x01000001>, /* SMBB_CHG */
+					 <0x01100002>, /* SMBB_BUCK */
+					 <0x01200003>, /* SMBB_BIF */
+					 <0x01300004>, /* SMBB_USB */
+					 <0x01500005>, /* SMBB_BOOST */
+					 <0x01600006>, /* SMBB_MISC */
+					 <0x02800009>, /* COINCELL */
+					 <0x02c000a6>, /* MBG */
+					 <0x0310000a>, /* VADC1_USR */
+					 <0x03200041>, /* VADC1_MDM */
+					 <0x0330000b>, /* VADC1_BMS */
+					 <0x0340000c>, /* VADC2_BTM */
+					 <0x0360000d>, /* IADC1_USR */
+					 <0x03700042>, /* IADC1_MDM */
+					 <0x0380000e>, /* IADC1_BMS */
+					 <0x0400000f>, /* BMS_1 */
+					 <0x050000a7>, /* SHARED_XO */
+					 <0x051000a8>, /* BB_CLK1 */
+					 <0x05200010>, /* BB_CLK2 */
+					 <0x05a000ac>, /* SLEEP_CLK */
+					 <0x06000045>, /* RTC_RW */
+					 <0x06100012>, /* RTC_ALARM */
+					 <0x070000ae>, /* PBS_CORE */
+					 <0x071000af>, /* PBS_CLIENT_1 */
+					 <0x072000b0>, /* PBS_CLIENT_2 */
+					 <0x073000b1>, /* PBS_CLIENT_3 */
+					 <0x074000b2>, /* PBS_CLIENT_4 */
+					 <0x075000b3>, /* PBS_CLIENT_5 */
+					 <0x076000b4>, /* PBS_CLIENT_6 */
+					 <0x07700046>, /* PBS_CLIENT_7 */
+					 <0x07800047>, /* PBS_CLIENT_8 */
+					 <0x079000b5>, /* PBS_CLIENT_9 */
+					 <0x07a000b6>, /* PBS_CLIENT_10 */
+					 <0x07b000b7>, /* PBS_CLIENT_11 */
+					 <0x07c000b8>, /* PBS_CLIENT_12 */
+					 <0x07d000b9>, /* PBS_CLIENT_13 */
+					 <0x07e000ba>, /* PBS_CLIENT_14 */
+					 <0x07f000bb>, /* PBS_CLIENT_15 */
+					 <0x0a0000bd>, /* MPP_1 */
+					 <0x0a100014>, /* MPP_2 */
+					 <0x0a200015>, /* MPP_3 */
+					 <0x0a300016>, /* MPP_4 */
+					 <0x0a400048>, /* MPP_5 */
+					 <0x0a500017>, /* MPP_6 */
+					 <0x0a600018>, /* MPP_7 */
+					 <0x0a700049>, /* MPP_8 */
+					 <0x0c000019>, /* GPIO_1 */
+					 <0x0c10001a>, /* GPIO_2 */
+					 <0x0c20004a>, /* GPIO_3 */
+					 <0x0c30004b>, /* GPIO_4 */
+					 <0x0c40001b>, /* GPIO_5 */
+					 <0x0c50001c>, /* GPIO_6 */
+					 <0x0c60001d>, /* GPIO_7 */
+					 <0x0c70004c>, /* GPIO_8 */
+					 <0x0fe000be>, /* TRIM_0 */
+					 <0x110000bf>, /* BUCK_CMN_1 */
+					 <0x114000c0>, /* SMPS1 */
+					 <0x115000c1>, /* FTPS1_1 */
+					 <0x116000c2>, /* BUCK_FREQ_1 */
+					 <0x1170001e>, /* SMPS2 */
+					 <0x1180001f>, /* FTPS1_2 */
+					 <0x11900020>, /* BUCK_FREQ_2 */
+					 <0x11a000c3>, /* SMPS3 */
+					 <0x11b000c4>, /* SMPS_3_PS1 */
+					 <0x11c000c5>, /* BUCK_FREQ_3 */
+					 <0x11d000c6>, /* SMPS4 */
+					 <0x11e000c7>, /* SMPS_4_PS1 */
+					 <0x11f000c8>, /* BUCK_FREQ_4 */
+					 <0x120000c9>, /* SMPS5 */
+					 <0x121000ca>, /* SMPS_5_PS1 */
+					 <0x122000cb>, /* BUCK_FREQ_5 */
+					 <0x140000cc>, /* LDO_1 */
+					 <0x141000cd>, /* LDO_2 */
+					 <0x142000ce>, /* LDO_3 */
+					 <0x143000cf>, /* LDO_4 */
+					 <0x144000d0>, /* LDO_5 */
+					 <0x145000d1>, /* LDO_6 */
+					 <0x146000d2>, /* LDO_7 */
+					 <0x147000d3>, /* LDO_8 */
+					 <0x148000d4>, /* LDO_9 */
+					 <0x149000d5>, /* LDO_10 */
+					 <0x14a000d6>, /* LDO_11 */
+					 <0x14b000d7>, /* LDO_12 */
+					 <0x14c000d8>, /* LDO_13 */
+					 <0x14d000d9>, /* LDO_14 */
+					 <0x14e000da>, /* LDO_15 */
+					 <0x14f000db>, /* LDO_16 */
+					 <0x150000dc>, /* LDO_17 */
+					 <0x151000dd>, /* LDO_18 */
+					 <0x152000de>, /* LDO_19 */
+					 <0x153000df>, /* LDO_20 */
+					 <0x154000e0>, /* LDO_21 */
+					 <0x155000e1>, /* LDO_22 */
+					 <0x156000e2>, /* LDO_23 */
+					 <0x157000e3>, /* LDO_24 */
+					 <0x158000e4>, /* LDO_25 */
+					 <0x159000e5>, /* LDO_26 */
+					 <0x15a000e6>, /* LDO_27 */
+					 <0x15b000e7>, /* LDO_28 */
+					 <0x180000e8>, /* LVS_1 */
+					 <0x1b0000e9>, /* LPG_LUT */
+					 <0x1b1000ea>, /* LPG_CHAN_1 */
+					 <0x1b200023>, /* LPG_CHAN_2 */
+					 <0x1b300024>, /* LPG_CHAN_3 */
+					 <0x1b400025>, /* LPG_CHAN_4 */
+					 <0x1b500026>, /* LPG_CHAN_5 */
+					 <0x1b600027>, /* LPG_CHAN_6 */
+					 <0x1bc00028>, /* PWM_3D */
+					 <0x1c000029>, /* VIB1 */
+					 <0x1d30002a>, /* FLASH_DRV */
+					 <0x1d80002b>; /* WLED */
+	};
+
 };
 
 &gdsc_venus {
@@ -178,3 +346,4 @@
 };
 
 /include/ "msm8226-regulator.dtsi"
+/include/ "msm-pm8026.dtsi"
diff --git a/arch/arm/boot/dts/msm8910.dtsi b/arch/arm/boot/dts/msm8910.dtsi
index 3786b02..8ad4eda 100644
--- a/arch/arm/boot/dts/msm8910.dtsi
+++ b/arch/arm/boot/dts/msm8910.dtsi
@@ -11,6 +11,7 @@
  */
 
 /include/ "skeleton.dtsi"
+/include/ "msm-iommu-v1.dtsi"
 /include/ "msm8910-ion.dtsi"
 /include/ "msm-gdsc.dtsi"
 
@@ -32,7 +33,9 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		gpio-controller;
 		#gpio-cells = <2>;
+		interrupts = <0 208 0>;
 	};
 
 	timer {
@@ -70,10 +73,12 @@
 	sdcc1: qcom,sdcc@f9824000 {
 		cell-index = <1>; /* SDC1 eMMC slot */
 		compatible = "qcom,msm-sdcc";
-		reg = <0xf9824000 0x800>;
-		reg-names = "core_mem";
-		interrupts = <0 123 0>;
-		interrupt-names = "core_irq";
+		reg = <0xf9824000 0x800>,
+		      <0xf9824800 0x100>,
+		      <0xf9804000 0x7000>;
+		reg-names = "core_mem", "dml_mem", "bam_mem";
+		interrupts = <0 123 0>, <0 137 0>;
+		interrupt-names = "core_irq", "bam_irq";
 
 		vdd-supply = <&pm8110_l17>;
 		qcom,vdd-always-on;
@@ -102,10 +107,12 @@
 	sdcc2: qcom,sdcc@f98a4000 {
 		cell-index = <2>; /* SDC2 SD card slot */
 		compatible = "qcom,msm-sdcc";
-		reg = <0xf98a4000 0x800>;
-		reg-names = "core_mem";
-		interrupts = <0 125 0>;
-		interrupt-names = "core_irq";
+		reg = <0xf98a4000 0x800>,
+		      <0xf98a4800 0x100>,
+		      <0xf9884000 0x7000>;
+		reg-names = "core_mem", "dml_mem", "bam_mem";
+		interrupts = <0 125 0>, <0 220 0>;
+		interrupt-names = "core_irq", "bam_irq";
 
 		vdd-supply = <&pm8110_l18>;
 		qcom,vdd-voltage-level = <2950000 2950000>;
@@ -133,9 +140,9 @@
 		qcom,device-type = <3>;
 	};
 
-	qcom,smem@fa00000 {
+	qcom,smem@d600000 {
 		compatible = "qcom,smem";
-		reg = <0xfa00000 0x200000>,
+		reg = <0xd600000 0x200000>,
 			<0xfa006000 0x1000>,
 			<0xfc428000 0x4000>;
 		reg-names = "smem", "irq-reg-base", "aux-mem1";
@@ -209,6 +216,96 @@
 		qcom,pet-time = <10000>;
 		qcom,ipi-ping = <1>;
 	};
+
+	spmi_bus: qcom,spmi@fc4c0000 {
+		cell-index = <0>;
+		compatible = "qcom,spmi-pmic-arb";
+		reg = <0xfc4cf000 0x1000>,
+		      <0Xfc4cb000 0x1000>;
+		/* 190,ee0_krait_hlos_spmi_periph_irq */
+		/* 187,channel_0_krait_hlos_trans_done_irq */
+		interrupts = <0 190 0>, <0 187 0>;
+		qcom,not-wakeup;
+		qcom,pmic-arb-ee = <0>;
+		qcom,pmic-arb-channel = <0>;
+		qcom,pmic-arb-ppid-map = <0x001000a0>, /* PM8110 */
+					 <0x005000a2>, /* INTERRUPT */
+					 <0x006000a3>, /* SPMI_0 */
+					 <0x00800000>, /* PON0 */
+					 <0x01000002>, /* SMBB_CHG */
+					 <0x01100003>, /* SMBB_BUCK */
+					 <0x01200004>, /* SMBB_BIF */
+					 <0x01300005>, /* SMBB_USB */
+					 <0x01500006>, /* SMBB_BOOST */
+					 <0x01600007>, /* SMBB_MISC */
+					 <0x0280000a>, /* COINCELL */
+					 <0x02c000a5>, /* MBG */
+					 <0x0310000b>, /* VADC1_LC_USR */
+					 <0x03200041>, /* VADC3_LC_MDM */
+					 <0x0330000c>, /* VADC3_LC_BMS */
+					 <0x0340000d>, /* VADC2_LC_BTM */
+					 <0x0360000e>, /* IADC1_USR */
+					 <0x03700042>, /* IADC2_MDM */
+					 <0x0380000f>, /* IADC2_BMS */
+					 <0x04000010>, /* BMS_1 */
+					 <0x050000a6>, /* SHARED_XO */
+					 <0x051000a7>, /* BB_CLK1 */
+					 <0x054000a8>, /* RF_CLK1 */
+					 <0x055000a9>, /* RF_CLK1 */
+					 <0x05a000ab>, /* SLEEP_CLK */
+					 <0x06000043>, /* RTC_RW */
+					 <0x06100011>, /* RTC_ALARM */
+					 <0x070000ac>, /* PBS_CORE */
+					 <0x071000ad>, /* PBS_CLIENT_1 */
+					 <0x072000ae>, /* PBS_CLIENT_2 */
+					 <0x07300013>, /* PBS_CLIENT_3 */
+					 <0x07400044>, /* PBS_CLIENT_4 */
+					 <0x0a000014>, /* MPP_1 */
+					 <0x0a100015>, /* MPP_2 */
+					 <0x0a200016>, /* MPP_3 */
+					 <0x0a300045>, /* MPP_4 */
+					 <0x0c000046>, /* GPIO_1 */
+					 <0x0c1000f0>, /* GPIO_2 */
+					 <0x0c2000af>, /* GPIO_3 */
+					 <0x0c300047>, /* GPIO_4 */
+					 <0x0fe000b0>, /* TRIM_0 */
+					 <0x110000b1>, /* BUCK_CMN_1 */
+					 <0x11400048>, /* SMPS1 */
+					 <0x115000b2>, /* SMPS_1_PS1 */
+					 <0x116000b3>, /* BUCK_FREQ_1 */
+					 <0x11700017>, /* SMPS2 */
+					 <0x11800018>, /* FTPS1_2 */
+					 <0x11900019>, /* BUCK_FREQ_2 */
+					 <0x11a000b4>, /* SMPS3 */
+					 <0x11b000b5>, /* SMPS_3_PS1 */
+					 <0x11c000b6>, /* BUCK_FREQ_3 */
+					 <0x11d000b7>, /* SMPS4 */
+					 <0x11e000b8>, /* SMPS_4_PS1 */
+					 <0x11f000b9>, /* BUCK_FREQ_4 */
+					 <0x140000ba>, /* LDO_1 */
+					 <0x141000bb>, /* LDO_2 */
+					 <0x142000bc>, /* LDO_3 */
+					 <0x143000bd>, /* LDO_4 */
+					 <0x144000be>, /* LDO_5 */
+					 <0x145000bf>, /* LDO_6 */
+					 <0x146000c0>, /* LDO_7 */
+					 <0x147000c1>, /* LDO_8 */
+					 <0x148000c2>, /* LDO_9 */
+					 <0x149000c3>, /* LDO_10 */
+					 <0x14a000c4>, /* LDO_11 */
+					 <0x14b000c5>, /* LDO_12 */
+					 <0x14c000c6>, /* LDO_13 */
+					 <0x14d000c7>, /* LDO_14 */
+					 <0x14e000c8>, /* LDO_15 */
+					 <0x14f000c9>, /* LDO_16 */
+					 <0x150000ca>, /* LDO_17 */
+					 <0x151000cb>, /* LDO_18 */
+					 <0x152000cc>, /* LDO_19 */
+					 <0x153000cd>, /* LDO_20 */
+					 <0x154000ce>, /* LDO_21 */
+					 <0x155000cf>; /* LDO_22 */
+	};
+
 };
 
 &gdsc_vfe {
@@ -219,4 +316,29 @@
 	status = "ok";
 };
 
+&lpass_iommu {
+	status = "ok";
+};
+
+&copss_iommu {
+	status = "ok";
+};
+
+&mdpe_iommu {
+	status = "ok";
+};
+
+&mdps_iommu {
+	status = "ok";
+};
+
+&gfx_iommu {
+	status = "ok";
+};
+
+&vfe_iommu {
+	status = "ok";
+};
+
 /include/ "msm8910-regulator.dtsi"
+/include/ "msm-pm8110.dtsi"
diff --git a/arch/arm/boot/dts/msm8974-cdp.dtsi b/arch/arm/boot/dts/msm8974-cdp.dtsi
index bd02d89..6aaf677 100644
--- a/arch/arm/boot/dts/msm8974-cdp.dtsi
+++ b/arch/arm/boot/dts/msm8974-cdp.dtsi
@@ -156,6 +156,7 @@
 
 	sound {
 		qcom,model = "msm8974-taiko-cdp-snd-card";
+		qcom,hdmi-audio-rx;
 	};
 };
 
@@ -233,6 +234,45 @@
 	wp-gpios = <&pm8941_gpios 29 0x1>;
 };
 
+&usb3 {
+	qcom,otg-capability;
+};
+
+&pm8941_chg {
+	status = "ok";
+
+	qcom,chg-charging-disabled;
+	qcom,chg-use-default-batt-values;
+
+	qcom,chg-chgr@1000 {
+		status = "ok";
+	};
+
+	qcom,chg-buck@1100 {
+		status = "ok";
+	};
+
+	qcom,chg-bat-if@1200 {
+		status = "ok";
+	};
+
+	qcom,chg-usb-chgpth@1300 {
+		status = "ok";
+	};
+
+	qcom,chg-dc-chgpth@1400 {
+		status = "ok";
+	};
+
+	qcom,chg-boost@1500 {
+		status = "ok";
+	};
+
+	qcom,chg-misc@1600 {
+		status = "ok";
+	};
+};
+
 &pm8941_gpios {
 	gpio@c000 { /* GPIO 1 */
 	};
diff --git a/arch/arm/boot/dts/msm8974-fluid.dtsi b/arch/arm/boot/dts/msm8974-fluid.dtsi
index 8479dfa..a027a1e 100644
--- a/arch/arm/boot/dts/msm8974-fluid.dtsi
+++ b/arch/arm/boot/dts/msm8974-fluid.dtsi
@@ -172,6 +172,7 @@
 
 	sound {
 		qcom,model = "msm8974-taiko-fluid-snd-card";
+		qcom,hdmi-audio-rx;
 	};
 };
 
@@ -252,6 +253,44 @@
 	cd-gpios = <&msmgpio 62 0x1>;
 };
 
+&usb3 {
+	qcom,otg-capability;
+};
+
+&pm8941_chg {
+	status = "ok";
+
+	qcom,chg-charging-disabled;
+
+	qcom,chg-chgr@1000 {
+		status = "ok";
+	};
+
+	qcom,chg-buck@1100 {
+		status = "ok";
+	};
+
+	qcom,chg-bat-if@1200 {
+		status = "ok";
+	};
+
+	qcom,chg-usb-chgpth@1300 {
+		status = "ok";
+	};
+
+	qcom,chg-dc-chgpth@1400 {
+		status = "ok";
+	};
+
+	qcom,chg-boost@1500 {
+		status = "ok";
+	};
+
+	qcom,chg-misc@1600 {
+		status = "ok";
+	};
+};
+
 &pm8941_gpios {
 	gpio@c000 { /* GPIO 1 */
 	};
@@ -457,3 +496,22 @@
 	mpp@a300 { /* MPP 4 */
 	};
 };
+
+&spi_epm {
+	epm-adc@0 {
+		compatible = "cy,epm-adc-cy8c5568lti-114";
+		reg = <0>;
+		interrupt-parent = <&msmgpio>;
+		spi-max-frequency = <960000>;
+		qcom,channels = <31>;
+		qcom,gain = <100 100 100 50 100 100 1 100 1 50
+				1 100 1 100 50 50 50 50 50 50
+				100 50 100 50 50 50 50 50 50 50
+				50>;
+		qcom,rsense = <2 2 2 200 20 2 1 2 1 30
+				1 10 1 30 50 30 500 30 100 30
+				100 500 20 200 1000 20 1000 1000 70 200
+				50>;
+		qcom,channel-type = <0x1540>;
+	};
+};
diff --git a/arch/arm/boot/dts/msm8974-gpu.dtsi b/arch/arm/boot/dts/msm8974-gpu.dtsi
index 6623568..480a034 100644
--- a/arch/arm/boot/dts/msm8974-gpu.dtsi
+++ b/arch/arm/boot/dts/msm8974-gpu.dtsi
@@ -36,8 +36,8 @@
 		qcom,msm-bus,num-paths = <2>;
 		qcom,msm-bus,vectors-KBps =
 				<26 512 0 0>, <89 604 0 0>,
-				<26 512 0 2000000>, <89 604 0 3000000>,
-				<26 512 0 4000000>, <89 604 0 5000000>,
+				<26 512 0 1600000>, <89 604 0 3000000>,
+				<26 512 0 4000000>, <89 604 0 4500000>,
 				<26 512 0 6400000>, <89 604 0 7600000>;
 
 		/* GDSC oxili regulators */
@@ -108,7 +108,7 @@
 			qcom,algo-ss-win-size-min-us = <1000000>;
 			qcom,algo-ss-win-size-max-us = <1000000>;
 			qcom,algo-ss-util-pct = <95>;
-			qcom,algo-ss-iobusy-conv = <100>;
+			qcom,algo-ss-no-corr-below-freq = <0>;
 
 			qcom,energy-active-coeff-a = <2492>;
 			qcom,energy-active-coeff-b = <0>;
diff --git a/arch/arm/boot/dts/msm8974-liquid.dtsi b/arch/arm/boot/dts/msm8974-liquid.dtsi
index f412d177..81f8c09 100644
--- a/arch/arm/boot/dts/msm8974-liquid.dtsi
+++ b/arch/arm/boot/dts/msm8974-liquid.dtsi
@@ -229,21 +229,49 @@
 			"MIC BIAS2 External", "ANCRight Headset Mic",
 			"AMIC4", "MIC BIAS2 External",
 			"MIC BIAS2 External", "ANCLeft Headset Mic",
-			"DMIC1", "MIC BIAS1 External",
+			"DMIC1", "MIC BIAS3 External",
 			"MIC BIAS1 External", "Digital Mic1",
-			"DMIC2", "MIC BIAS1 External",
+			"DMIC2", "MIC BIAS3 External",
 			"MIC BIAS1 External", "Digital Mic2",
-			"DMIC3", "MIC BIAS3 External",
+			"DMIC3", "MIC BIAS2 External",
 			"MIC BIAS3 External", "Digital Mic3",
 			"DMIC4", "MIC BIAS3 External",
 			"MIC BIAS3 External", "Digital Mic4",
-			"DMIC5", "MIC BIAS4 External",
+			"DMIC5", "MIC BIAS2 External",
 			"MIC BIAS4 External", "Digital Mic5",
-			"DMIC6", "MIC BIAS4 External",
+			"DMIC6", "MIC BIAS2 External",
 			"MIC BIAS4 External", "Digital Mic6";
 
 		qcom,ext-spk-amp-supply = <&ext_5v>;
 		qcom,ext-spk-amp-gpio = <&pm8841_mpps 1 0>;
+
+		qcom,hdmi-audio-rx;
+	};
+
+	hsic_hub {
+		compatible = "qcom,hsic-smsc-hub";
+		#address-cells = <1>;
+		#size-cells = <1>;
+		ranges;
+		smsc,reset-gpio = <&pm8941_gpios 8 0x00>;
+		smsc,refclk-gpio = <&pm8941_gpios 16 0x00>;
+		smsc,int-gpio = <&msmgpio 50 0x00>;
+		hub_int-supply = <&pm8941_l10>;
+		hub_vbus-supply = <&ext_5v>;
+
+		hsic@f9a00000 {
+			compatible = "qcom,hsic-host";
+			reg = <0xf9a00000 0x400>;
+			interrupts = <0 136 0>;
+			interrupt-names = "core_irq";
+			HSIC_VDDCX-supply = <&pm8841_s2>;
+			HSIC_GDSC-supply = <&gdsc_usb_hsic>;
+			hsic,strobe-gpio = <&msmgpio 144 0x00>;
+			hsic,data-gpio = <&msmgpio 145 0x00>;
+			hsic,ignore-cal-pad-config;
+			hsic,strobe-pad-offset = <0x2050>;
+			hsic,data-pad-offset = <0x2054>;
+		};
 	};
 };
 
@@ -294,6 +322,11 @@
 	};
 
 	gpio@c700 { /* GPIO 8 */
+		/* HSIC_HUB-RESET */
+		qcom,mode = <1>;		/* DIG_OUT */
+		qcom,pull = <5>;		/* PULL_NO */
+		qcom,out-strength = <2>;	/* STRENGTH_MED */
+		qcom,master-en = <1>;
 	};
 
 	gpio@c800 { /* GPIO 9 */
@@ -341,6 +374,14 @@
 	};
 
 	gpio@cf00 { /* GPIO 16 */
+		/* HSIC_HUB-INT_N */
+		qcom,mode = <1>;
+		qcom,output-type = <0>;
+		qcom,pull = <5>;
+		qcom,vin-sel = <2>;
+		qcom,out-strength = <3>;
+		qcom,src-sel = <2>;
+		qcom,master-en = <1>;
 	};
 
 	gpio@d000 { /* GPIO 17 */
@@ -520,3 +561,22 @@
 		qcom,master-en = <1>; /* ENABLE MPP */
 	};
 };
+
+&spi_epm {
+	epm-adc@0 {
+		compatible = "cy,epm-adc-cy8c5568lti-114";
+		reg = <0>;
+		interrupt-parent = <&msmgpio>;
+		spi-max-frequency = <960000>;
+		qcom,channels = <31>;
+		qcom,gain = <50 50 50 50 50 100 50 50 50 50
+				50 50 50 50 100 50 50 50 50 100
+				50 50 50 100 50 50 50 1 1 1
+				1>;
+		qcom,rsense = <40 10 10 25 10 1000 75 25 10 25
+				33 500 200 10 500 100 33 200 25 100
+				75 500 50 200 5 5 3 1 1 1
+				1>;
+		qcom,channel-type = <0xf0000000>;
+	};
+};
diff --git a/arch/arm/boot/dts/msm8974-mdss.dtsi b/arch/arm/boot/dts/msm8974-mdss.dtsi
index b765611..d0cec3a 100644
--- a/arch/arm/boot/dts/msm8974-mdss.dtsi
+++ b/arch/arm/boot/dts/msm8974-mdss.dtsi
@@ -39,19 +39,24 @@
 			<0xfc4b8000 0x60F0>;
 		reg-names = "core_physical", "phy_physical", "qfprom_physical";
 
+		hpd-gdsc-supply = <&gdsc_mdss>;
 		hpd-5v-supply = <&pm8941_mvs2>;
 		core-vdda-supply = <&pm8941_l12>;
 		core-vcc-supply = <&pm8941_s3>;
-		qcom,hdmi-tx-supply-names = "hpd-5v", "core-vdda", "core-vcc";
-		qcom,hdmi-tx-supply-type = <1 0 0>;
-		qcom,hdmi-tx-min-voltage-level = <0 1800000 1800000>;
-		qcom,hdmi-tx-max-voltage-level = <0 1800000 1800000>;
-		qcom,hdmi-tx-op-mode = <0 1800000 0>;
+		qcom,hdmi-tx-supply-names = "hpd-gdsc", "hpd-5v", "core-vdda", "core-vcc";
+		qcom,hdmi-tx-supply-type = <1 1 0 0>;
+		qcom,hdmi-tx-min-voltage-level = <0 0 1800000 1800000>;
+		qcom,hdmi-tx-max-voltage-level = <0 0 1800000 1800000>;
+		qcom,hdmi-tx-op-mode = <0 0 1800000 0>;
 
 		qcom,hdmi-tx-cec = <&msmgpio 31 0>;
 		qcom,hdmi-tx-ddc-clk = <&msmgpio 32 0>;
 		qcom,hdmi-tx-ddc-data = <&msmgpio 33 0>;
 		qcom,hdmi-tx-hpd = <&msmgpio 34 0>;
+
+		qcom,msm-hdmi-audio-rx {
+			compatible = "qcom,msm-hdmi-audio-codec-rx";
+		};
 	};
 
 	qcom,mdss_wb_panel {
diff --git a/arch/arm/boot/dts/msm8974-mtp.dtsi b/arch/arm/boot/dts/msm8974-mtp.dtsi
index 9fb7d0e..48bb5ba 100644
--- a/arch/arm/boot/dts/msm8974-mtp.dtsi
+++ b/arch/arm/boot/dts/msm8974-mtp.dtsi
@@ -476,3 +476,22 @@
 		qcom,cdc-micbias2-ext-cap;
 	};
 };
+
+&spi_epm {
+	epm-adc@0 {
+		compatible = "cy,epm-adc-cy8c5568lti-114";
+		reg = <0>;
+		interrupt-parent = <&msmgpio>;
+		spi-max-frequency = <960000>;
+		qcom,channels = <31>;
+		qcom,gain = <100 100 100 50 100 100 1 100 1 50
+				1 100 1 100 50 50 50 50 50 50
+				100 50 100 50 50 50 50 50 50 50
+				50>;
+		qcom,rsense = <2 2 2 200 20 2 1 2 1 30
+				1 10 1 30 50 30 500 30 100 30
+				100 500 20 200 1000 20 1000 1000 70 200
+				50>;
+		qcom,channel-type = <0x1540>;
+	};
+};
diff --git a/arch/arm/boot/dts/msm8974-pm.dtsi b/arch/arm/boot/dts/msm8974-pm.dtsi
index 52f2a41..b8a977b 100644
--- a/arch/arm/boot/dts/msm8974-pm.dtsi
+++ b/arch/arm/boot/dts/msm8974-pm.dtsi
@@ -143,6 +143,7 @@
 			qcom,type = <0x62706d73>;	/* "smpb" */
 			qcom,id = <0x02>;
 			qcom,key = <0x6e726f63>;	/* "corn" */
+			qcom,init-value = <5>;		/* Super Turbo */
 		};
 
 		qcom,lpm-resources@1 {
@@ -152,6 +153,7 @@
 			qcom,type = <0x62706d73>;	/* "smpb" */
 			qcom,id = <0x01>;
 			qcom,key = <0x7675>;		/* "uv" */
+			qcom,init-value = <1050000>;	/* Super Turbo */
 		};
 
 		qcom,lpm-resources@2 {
@@ -161,12 +163,14 @@
 			qcom,type = <0x306b6c63>;	/* "clk0" */
 			qcom,id = <0x00>;
 			qcom,key = <0x62616e45>;	/* "Enab" */
+			qcom,init-value = <1>;		/* On */
 		};
 
 		qcom,lpm-resources@3 {
 			reg = <0x3>;
 			qcom,name = "l2";
 			qcom,resource-type = <1>;
+			qcom,init-value = <2>;		/* Retention */
 		};
 	};
 
@@ -184,10 +188,10 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <100>;
-			qcom,ss-power = <650>;
-			qcom,energy-overhead = <801>;
-			qcom,time-overhead = <200>;
+			qcom,latency-us = <1>;
+			qcom,ss-power = <784>;
+			qcom,energy-overhead = <190000>;
+			qcom,time-overhead = <100>;
 		};
 
 		qcom,lpm-level@1 {
@@ -199,10 +203,10 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <1500>;
-			qcom,ss-power = <200>;
-			qcom,energy-overhead = <576000>;
-			qcom,time-overhead = <2000>;
+			qcom,latency-us = <75>;
+			qcom,ss-power = <735>;
+			qcom,energy-overhead = <77341>;
+			qcom,time-overhead = <105>;
 		};
 
 
@@ -215,10 +219,10 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <2000>;
-			qcom,ss-power = <200>;
-			qcom,energy-overhead = <576000>;
-			qcom,time-overhead = <2000>;
+			qcom,latency-us = <95>;
+			qcom,ss-power = <725>;
+			qcom,energy-overhead = <99500>;
+			qcom,time-overhead = <130>;
 		};
 
 		qcom,lpm-level@3 {
@@ -230,10 +234,10 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <8500>;
-			qcom,ss-power = <51>;
-			qcom,energy-overhead = <1122000>;
-			qcom,time-overhead = <8500>;
+			qcom,latency-us = <2000>;
+			qcom,ss-power = <138>;
+			qcom,energy-overhead = <1208400>;
+			qcom,time-overhead = <3200>;
 		};
 
 		qcom,lpm-level@4 {
@@ -241,33 +245,18 @@
 			qcom,mode = <3>;        /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
 			qcom,xo = <1>;          /* ON */
 			qcom,l2 = <0>;          /* OFF */
-			qcom,vdd-mem-upper-bound = <1150000>; /* MAX */
-			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
-			qcom,vdd-dig-upper-bound = <5>; /* MAX */
-			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <9000>;
-			qcom,ss-power = <51>;
-			qcom,energy-overhead = <1130300>;
-			qcom,time-overhead = <9000>;
-		};
-
-		qcom,lpm-level@5 {
-			reg = <0x5>;
-			qcom,mode = <3>;        /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
-			qcom,xo = <1>;          /* ON */
-			qcom,l2 = <0>;          /* OFF */
 			qcom,vdd-mem-upper-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-mem-lower-bound = <750000>;  /* RETENTION HIGH */
 			qcom,vdd-dig-upper-bound = <3>;  /* ACTIVE */
 			qcom,vdd-dig-lower-bound = <2>;  /* RETENTION HIGH */
-			qcom,latency-us = <10000>;
-			qcom,ss-power = <51>;
-			qcom,energy-overhead = <1130300>;
-			qcom,time-overhead = <10000>;
+			qcom,latency-us = <3000>;
+			qcom,ss-power = <110>;
+			qcom,energy-overhead = <1250300>;
+			qcom,time-overhead = <3500>;
 		};
 
-		qcom,lpm-level@6 {
-			reg = <0x6>;
+		qcom,lpm-level@5 {
+			reg = <0x5>;
 			qcom,mode = <3>;        /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
 			qcom,xo = <0>;          /* OFF */
 			qcom,l2 = <1>;          /* GDHS */
@@ -275,14 +264,14 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <12000>;
-			qcom,ss-power = <14>;
-			qcom,energy-overhead = <2205900>;
-			qcom,time-overhead = <12000>;
+			qcom,latency-us = <3000>;
+			qcom,ss-power = <68>;
+			qcom,energy-overhead = <1350200>;
+			qcom,time-overhead = <4000>;
 		};
 
-		qcom,lpm-level@7 {
-			reg = <0x7>;
+		qcom,lpm-level@6 {
+			reg = <0x6>;
 			qcom,mode = <3>;        /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
 			qcom,xo = <0>;          /* OFF */
 			qcom,l2 = <0>;          /* OFF */
@@ -290,14 +279,14 @@
 			qcom,vdd-mem-lower-bound = <1050000>; /* ACTIVE */
 			qcom,vdd-dig-upper-bound = <5>; /* MAX */
 			qcom,vdd-dig-lower-bound = <3>;  /* ACTIVE */
-			qcom,latency-us = <18000>;
-			qcom,ss-power = <12>;
-			qcom,energy-overhead = <2364250>;
-			qcom,time-overhead = <18000>;
+			qcom,latency-us = <10300>;
+			qcom,ss-power = <63>;
+			qcom,energy-overhead = <2128000>;
+			qcom,time-overhead = <18200>;
 		};
 
-		qcom,lpm-level@8 {
-			reg = <0x8>;
+		qcom,lpm-level@7 {
+			reg = <0x7>;
 			qcom,mode= <3>;         /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
 			qcom,xo = <0>;          /* OFF */
 			qcom,l2 = <0>;          /* OFF */
@@ -305,14 +294,14 @@
 			qcom,vdd-mem-lower-bound = <750000>;  /* RETENTION HIGH */
 			qcom,vdd-dig-upper-bound = <3>;  /* ACTIVE */
 			qcom,vdd-dig-lower-bound = <2>;  /* RETIONTION HIGH */
-			qcom,latency-us = <23500>;
+			qcom,latency-us = <18000>;
 			qcom,ss-power = <10>;
-			qcom,energy-overhead = <2667000>;
-			qcom,time-overhead = <23500>;
+			qcom,energy-overhead = <3202600>;
+			qcom,time-overhead = <27000>;
 		};
 
-		qcom,lpm-level@9 {
-			reg = <0x9>;
+		qcom,lpm-level@8 {
+			reg = <0x8>;
 			qcom,mode= <3>;         /* MSM_PM_SLEEP_MODE_POWER_COLLAPSE */
 			qcom,xo = <0>;          /* OFF */
 			qcom,l2 = <0>;          /* OFF */
@@ -320,10 +309,10 @@
 			qcom,vdd-mem-lower-bound = <750000>; /* RETENTION LOW */
 			qcom,vdd-dig-upper-bound = <2>; /* RETENTION HIGH */
 			qcom,vdd-dig-lower-bound = <0>; /* RETENTION LOW */
-			qcom,latency-us = <29700>;
-			qcom,ss-power = <5>;
-			qcom,energy-overhead = <2867000>;
-			qcom,time-overhead = <30000>;
+			qcom,latency-us = <20000>;
+			qcom,ss-power = <2>;
+			qcom,energy-overhead = <4252000>;
+			qcom,time-overhead = <32000>;
 		};
 	};
 
diff --git a/arch/arm/boot/dts/msm8974-regulator.dtsi b/arch/arm/boot/dts/msm8974-regulator.dtsi
index 2cef567..a7a7c88 100644
--- a/arch/arm/boot/dts/msm8974-regulator.dtsi
+++ b/arch/arm/boot/dts/msm8974-regulator.dtsi
@@ -108,9 +108,9 @@
 	rpm-regulator-smpb4 {
 		status = "okay";
 		pm8841_s4: regulator-s4 {
-			regulator-min-microvolt = <900000>;
+			regulator-min-microvolt = <815000>;
 			regulator-max-microvolt = <900000>;
-			qcom,init-voltage = <900000>;
+			qcom,init-voltage = <815000>;
 			status = "okay";
 		};
 	};
diff --git a/arch/arm/boot/dts/msm8974.dtsi b/arch/arm/boot/dts/msm8974.dtsi
index 6a7e81e..74b6521 100644
--- a/arch/arm/boot/dts/msm8974.dtsi
+++ b/arch/arm/boot/dts/msm8974.dtsi
@@ -40,6 +40,7 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		interrupts = <0 208 0>;
 	};
 
 	wcd9xxx_intc: wcd9xxx-irq {
@@ -188,8 +189,6 @@
 		qcom,vdd-voltage-level = <2950000 2950000>;
 		qcom,vdd-current-level = <9000 800000>;
 
-		qcom,vdd-io-always-on;
-		qcom,vdd-io-lpm-sup;
 		qcom,vdd-io-voltage-level = <1800000 2950000>;
 		qcom,vdd-io-current-level = <6 22000>;
 
@@ -322,7 +321,7 @@
 		qcom,bam-dma-res-pipes = <6>;
 	};
 
-	spi@f9966000 {
+	spi_epm: spi@f9966000 {
 		compatible = "qcom,spi-qup-v2";
 		cell-index = <7>;
 		reg = <0xf9966000 0x1000>;
@@ -334,23 +333,6 @@
 			<&msmgpio 54 0>, /* MISO */
 			<&msmgpio 53 0>; /* MOSI */
 		cs-gpios = <&msmgpio 55 0>;
-
-		epm-adc@0 {
-			compatible = "cy,epm-adc-cy8c5568lti-114";
-			reg = <0>;
-			interrupt-parent = <&msmgpio>;
-			spi-max-frequency = <960000>;
-			qcom,channels = <31>;
-			qcom,gain = <100 100 100 50 100 100 1 100 1 50
-					1 100 1 100 50 50 50 50 50 50
-					100 50 100 50 50 50 50 50 50 50
-					50>;
-			qcom,rsense = <2 2 2 200 20 2 1 2 1 30
-					1 10 1 30 50 30 500 30 100 30
-					100 500 20 200 1000 20 1000 1000 70 200
-					50>;
-			qcom,channel-type = <0x2a40>;
-		};
 	};
 
 	slim_msm: slim@fe12f000 {
@@ -407,7 +389,7 @@
 			qcom,cdc-micbias2-cfilt-sel = <0x1>;
 			qcom,cdc-micbias3-cfilt-sel = <0x2>;
 			qcom,cdc-micbias4-cfilt-sel = <0x2>;
-
+			qcom,cdc-mclk-clk-rate = <9600000>;
 			qcom,cdc-slim-ifd = "taiko-slim-ifd";
 			qcom,cdc-slim-ifd-elemental-addr = [00 00 A0 00 17 02];
 		};
@@ -820,6 +802,16 @@
 			compatible = "qcom,msm-dai-q6-dev";
 			qcom,msm-dai-q6-dev-id = <240>;
 		};
+
+		qcom,msm-dai-q6-incall-record-rx {
+			compatible = "qcom,msm-dai-q6-dev";
+			qcom,msm-dai-q6-dev-id = <32771>;
+		};
+
+		qcom,msm-dai-q6-incall-record-tx {
+			compatible = "qcom,msm-dai-q6-dev";
+			qcom,msm-dai-q6-dev-id = <32772>;
+		};
 	};
 
 	qcom,msm-auxpcm {
@@ -869,6 +861,14 @@
 			<11 604 32506 32506>;
 	};
 
+	qcom,msm-adsp-sensors {
+		compatible = "qcom,msm-adsp-sensors";
+		qcom,src-id = <11>;
+		qcom,dst-id = <604>;
+		qcom,ab = <32505856>;
+		qcom,ib = <32505856>;
+	};
+
 	qcom,mss@fc880000 {
 		compatible = "qcom,pil-q6v5-mss";
 		reg = <0xfc880000 0x100>,
diff --git a/arch/arm/boot/dts/msm9625-pm.dtsi b/arch/arm/boot/dts/msm9625-pm.dtsi
index 2839864..71fcfd6 100644
--- a/arch/arm/boot/dts/msm9625-pm.dtsi
+++ b/arch/arm/boot/dts/msm9625-pm.dtsi
@@ -42,6 +42,7 @@
 			qcom,type = <0x616F646C>;       /* "ldoa" */
 			qcom,id = <0x0A>;
 			qcom,key = <0x6e726f63>;	/* "corn" */
+			qcom,init-value = <5>;		/* Super Turbo */
 		};
 
 		qcom,lpm-resources@1 {
@@ -51,6 +52,7 @@
 			qcom,type = <0x616F646C>;       /* "ldoa" */
 			qcom,id = <0x0C>;
 			qcom,key =  <0x7675>;		/* "uv" */
+			qcom,init-value = <1050000>;	/* Super Turbo */
 		};
 
 		qcom,lpm-resources@2 {
@@ -60,6 +62,7 @@
 			qcom,type = <0x306b6c63>;	/* "clk0" */
 			qcom,id = <0x00>;
 			qcom,key = <0x62616e45>;	/* "Enab" */
+			qcom,init-value = <1>;		/* On */
 		};
 	};
 
@@ -174,7 +177,8 @@
 		qcom,ipc-bit-offset = <1>;
 
 		qcom,gic-parent = <&intc>;
-		qcom,gic-map = <41 172>, /* usb2_hsic_async_wakeup_irq */
+		qcom,gic-map = <47 172>, /* usb2_hsic_async_wakeup_irq */
+			<62 222>, /* ee0_krait_hlos_spmi_periph_irq */
 			<0xff 208>; /* summary_irq_kpss */
 
 		qcom,gpio-parent = <&msmgpio>;
diff --git a/arch/arm/boot/dts/msm9625.dtsi b/arch/arm/boot/dts/msm9625.dtsi
index cbd93df..6fee4e6 100644
--- a/arch/arm/boot/dts/msm9625.dtsi
+++ b/arch/arm/boot/dts/msm9625.dtsi
@@ -42,6 +42,7 @@
 		interrupt-controller;
 		#interrupt-cells = <2>;
 		reg = <0xfd510000 0x4000>;
+		interrupts = <0 208 0>;
 	};
 
 	timer: msm-qtimer@f9021000 {
@@ -86,6 +87,7 @@
 	android_usb@fc42b0c8 {
 		compatible = "qcom,android-usb";
 		reg = <0xfc42b0c8 0xc8>;
+		qcom,android-usb-swfi-latency = <100>;
 	};
 
 	hsic@f9a15000 {
@@ -416,6 +418,7 @@
 			qcom,cdc-micbias2-cfilt-sel = <0x1>;
 			qcom,cdc-micbias3-cfilt-sel = <0x2>;
 			qcom,cdc-micbias4-cfilt-sel = <0x2>;
+			qcom,cdc-mclk-clk-rate = <12288000>;
 		};
 
 		wcd9xxx_codec@77{
@@ -581,9 +584,9 @@
 /include/ "msm9625-regulator.dtsi"
 
 &pm8019_vadc {
-	chan@49 {
+	chan@31 {
 		label = "batt_id_therm";
-		qcom,channel-num = <49>;
+		reg = <0x31>;
 		qcom,decimation = <0>;
 		qcom,pre-div-channel-scaling = <0>;
 		qcom,calibration-type = "ratiometric";
@@ -592,9 +595,9 @@
 		qcom,fast-avg-setup = <0>;
 	};
 
-	chan@51 {
+	chan@33 {
 		label = "pa_therm1";
-		qcom,channel-num = <51>;
+		reg = <0x33>;
 		qcom,decimation = <0>;
 		qcom,pre-div-channel-scaling = <0>;
 		qcom,calibration-type = "ratiometric";
@@ -603,9 +606,9 @@
 		qcom,fast-avg-setup = <0>;
 	};
 
-	chan@52 {
+	chan@34 {
 		label = "pa_therm2";
-		qcom,channel-num = <52>;
+		reg = <0x34>;
 		qcom,decimation = <0>;
 		qcom,pre-div-channel-scaling = <0>;
 		qcom,calibration-type = "ratiometric";
@@ -614,9 +617,9 @@
 		qcom,fast-avg-setup = <0>;
 	};
 
-	chan@50 {
+	chan@32 {
 		label = "xo_therm";
-		qcom,channel-num = <50>;
+		reg = <0x32>;
 		qcom,decimation = <0>;
 		qcom,pre-div-channel-scaling = <0>;
 		qcom,calibration-type = "ratiometric";
@@ -625,9 +628,9 @@
 		qcom,fast-avg-setup = <0>;
 	};
 
-	chan@60 {
+	chan@3c {
 		label = "xo_therm_amux";
-		qcom,channel-num = <60>;
+		reg = <0x3c>;
 		qcom,decimation = <0>;
 		qcom,pre-div-channel-scaling = <0>;
 		qcom,calibration-type = "ratiometric";
diff --git a/arch/arm/configs/fsm9xxx-perf_defconfig b/arch/arm/configs/fsm9xxx-perf_defconfig
index 1dc853b..8a7928b 100644
--- a/arch/arm/configs/fsm9xxx-perf_defconfig
+++ b/arch/arm/configs/fsm9xxx-perf_defconfig
@@ -37,6 +37,8 @@
 # CONFIG_MSM_HW3D is not set
 # CONFIG_QSD_AUDIO is not set
 # CONFIG_SURF_FFA_GPIO_KEYPAD is not set
+CONFIG_MSM_SMCMOD=m
+CONFIG_MSM_SCM=y
 CONFIG_MSM_WATCHDOG=y
 CONFIG_MSM_RPC_PMIC=y
 CONFIG_MSM_RPC_USB=y
@@ -142,6 +144,8 @@
 # CONFIG_MFD_PM8XXX_MISC is not set
 CONFIG_REGULATOR=y
 CONFIG_REGULATOR_PM8058_XO=y
+CONFIG_ION=y
+CONFIG_ION_MSM=y
 # CONFIG_USB_SUPPORT is not set
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
diff --git a/arch/arm/configs/fsm9xxx_defconfig b/arch/arm/configs/fsm9xxx_defconfig
index 203d3b7..db2f25d 100644
--- a/arch/arm/configs/fsm9xxx_defconfig
+++ b/arch/arm/configs/fsm9xxx_defconfig
@@ -36,6 +36,8 @@
 # CONFIG_MSM_HW3D is not set
 # CONFIG_QSD_AUDIO is not set
 # CONFIG_SURF_FFA_GPIO_KEYPAD is not set
+CONFIG_MSM_SMCMOD=m
+CONFIG_MSM_SCM=y
 CONFIG_MSM_WATCHDOG=y
 CONFIG_MSM_RPC_PMIC=y
 CONFIG_MSM_RPC_USB=y
@@ -141,6 +143,8 @@
 # CONFIG_MFD_PM8XXX_MISC is not set
 CONFIG_REGULATOR=y
 CONFIG_REGULATOR_PM8058_XO=y
+CONFIG_ION=y
+CONFIG_ION_MSM=y
 # CONFIG_USB_SUPPORT is not set
 CONFIG_NEW_LEDS=y
 CONFIG_LEDS_CLASS=y
diff --git a/arch/arm/configs/msm7627a_defconfig b/arch/arm/configs/msm7627a_defconfig
index d403cec..27c10d0 100644
--- a/arch/arm/configs/msm7627a_defconfig
+++ b/arch/arm/configs/msm7627a_defconfig
@@ -376,7 +376,7 @@
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_DEBUG_SHIRQ=y
 CONFIG_LOCKUP_DETECTOR=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 CONFIG_DEBUG_SPINLOCK=y
 CONFIG_DEBUG_MUTEXES=y
diff --git a/arch/arm/configs/msm7630_defconfig b/arch/arm/configs/msm7630_defconfig
index 0e066d9..5964afb 100644
--- a/arch/arm/configs/msm7630_defconfig
+++ b/arch/arm/configs/msm7630_defconfig
@@ -369,7 +369,6 @@
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_DEBUG_FS=y
 CONFIG_LOCKUP_DETECTOR=y
-# CONFIG_SCHED_DEBUG is not set
 CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 CONFIG_DEBUG_SLAB=y
diff --git a/arch/arm/configs/msm8910_defconfig b/arch/arm/configs/msm8910_defconfig
index de78559..0fcfa97 100644
--- a/arch/arm/configs/msm8910_defconfig
+++ b/arch/arm/configs/msm8910_defconfig
@@ -47,6 +47,7 @@
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
+CONFIG_SCHED_MC=y
 CONFIG_ARM_ARCH_TIMER=y
 CONFIG_HOTPLUG_CPU=y
 CONFIG_PREEMPT=y
@@ -128,8 +129,7 @@
 CONFIG_MMC_BLOCK_MINORS=32
 CONFIG_MMC_TEST=m
 CONFIG_MMC_MSM=y
-CONFIG_SPS=y
-CONFIG_SPS_SUPPORT_NDP_BAM=y
+CONFIG_MMC_MSM_SPS_SUPPORT=y
 CONFIG_STAGING=y
 CONFIG_ANDROID=y
 CONFIG_ANDROID_BINDER_IPC=y
@@ -137,6 +137,9 @@
 CONFIG_ANDROID_RAM_CONSOLE=y
 CONFIG_ANDROID_TIMED_GPIO=y
 CONFIG_ANDROID_LOW_MEMORY_KILLER=y
+CONFIG_SPS=y
+CONFIG_SPS_SUPPORT_NDP_BAM=y
+CONFIG_MSM_IOMMU=y
 CONFIG_EXT2_FS=y
 CONFIG_EXT2_FS_XATTR=y
 CONFIG_EXT3_FS=y
@@ -151,7 +154,7 @@
 CONFIG_PRINTK_TIME=y
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_DEBUG_FS=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 # CONFIG_DEBUG_PREEMPT is not set
 CONFIG_DEBUG_INFO=y
diff --git a/arch/arm/configs/msm8960-perf_defconfig b/arch/arm/configs/msm8960-perf_defconfig
index 65ffa81..c95472f 100644
--- a/arch/arm/configs/msm8960-perf_defconfig
+++ b/arch/arm/configs/msm8960-perf_defconfig
@@ -89,6 +89,7 @@
 CONFIG_MSM_EBI_ERP=y
 CONFIG_MSM_CACHE_ERP=y
 CONFIG_MSM_L1_ERR_PANIC=y
+CONFIG_MSM_L1_RECOV_ERR_PANIC=y
 CONFIG_MSM_L1_ERR_LOG=y
 CONFIG_MSM_L2_ERP_2BIT_PANIC=y
 CONFIG_MSM_DCVS=y
@@ -100,6 +101,7 @@
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
 # CONFIG_SMP_ON_UP is not set
+CONFIG_SCHED_MC=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
 CONFIG_HIGHMEM=y
@@ -467,6 +469,7 @@
 CONFIG_MSM_SSBI=y
 CONFIG_SPS=y
 CONFIG_SPS_SUPPORT_BAMDMA=y
+CONFIG_MSM_AVTIMER=y
 CONFIG_MSM_IOMMU=y
 CONFIG_MOBICORE_SUPPORT=m
 CONFIG_MOBICORE_API=m
diff --git a/arch/arm/configs/msm8960_defconfig b/arch/arm/configs/msm8960_defconfig
index 01d1934..1842b6e 100644
--- a/arch/arm/configs/msm8960_defconfig
+++ b/arch/arm/configs/msm8960_defconfig
@@ -90,6 +90,7 @@
 CONFIG_MSM_EBI_ERP=y
 CONFIG_MSM_CACHE_ERP=y
 CONFIG_MSM_L1_ERR_PANIC=y
+CONFIG_MSM_L1_RECOV_ERR_PANIC=y
 CONFIG_MSM_L1_ERR_LOG=y
 CONFIG_MSM_L2_ERP_PRINT_ACCESS_ERRORS=y
 CONFIG_MSM_L2_ERP_1BIT_PANIC=y
@@ -105,6 +106,7 @@
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
 # CONFIG_SMP_ON_UP is not set
+CONFIG_SCHED_MC=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
 CONFIG_HIGHMEM=y
@@ -495,7 +497,7 @@
 CONFIG_PRINTK_TIME=y
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_LOCKUP_DETECTOR=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 CONFIG_DEBUG_KMEMLEAK=y
 CONFIG_DEBUG_KMEMLEAK_DEFAULT_OFF=y
diff --git a/arch/arm/configs/msm8974-perf_defconfig b/arch/arm/configs/msm8974-perf_defconfig
index bf44665..ad6dc6a 100644
--- a/arch/arm/configs/msm8974-perf_defconfig
+++ b/arch/arm/configs/msm8974-perf_defconfig
@@ -67,8 +67,11 @@
 CONFIG_MSM_OCMEM_LOCAL_POWER_CTRL=y
 CONFIG_MSM_OCMEM_DEBUG=y
 CONFIG_MSM_OCMEM_NONSECURE=y
+CONFIG_MSM_RTB=y
+CONFIG_MSM_RTB_SEPARATE_CPUS=y
 CONFIG_MSM_CACHE_ERP=y
 CONFIG_MSM_L1_ERR_PANIC=y
+CONFIG_MSM_L1_RECOV_ERR_PANIC=y
 CONFIG_MSM_L1_ERR_LOG=y
 CONFIG_MSM_L2_ERP_2BIT_PANIC=y
 CONFIG_MSM_ENABLE_WDOG_DEBUG_CONTROL=y
@@ -77,6 +80,7 @@
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
 # CONFIG_SMP_ON_UP is not set
+CONFIG_SCHED_MC=y
 CONFIG_ARM_ARCH_TIMER=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
@@ -218,8 +222,9 @@
 CONFIG_GENLOCK_MISCDEVICE=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_BLK_DEV_RAM=y
-CONFIG_TI_DRV2667=y
 CONFIG_QSEECOM=y
+CONFIG_USB_HSIC_SMSC_HUB=y
+CONFIG_TI_DRV2667=y
 CONFIG_SCSI=y
 CONFIG_SCSI_TGT=y
 CONFIG_BLK_DEV_SD=y
@@ -248,6 +253,7 @@
 CONFIG_INPUT_EVBUG=m
 CONFIG_KEYBOARD_GPIO=y
 CONFIG_INPUT_JOYSTICK=y
+CONFIG_JOYSTICK_XPAD=y
 CONFIG_INPUT_TOUCHSCREEN=y
 CONFIG_TOUCHSCREEN_ATMEL_MXT=y
 CONFIG_INPUT_MISC=y
@@ -282,7 +288,9 @@
 CONFIG_SENSORS_QPNP_ADC_CURRENT=y
 CONFIG_THERMAL=y
 CONFIG_THERMAL_TSENS8974=y
+CONFIG_THERMAL_MONITOR=y
 CONFIG_THERMAL_QPNP=y
+CONFIG_THERMAL_QPNP_ADC_TM=y
 CONFIG_WCD9320_CODEC=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_REGULATOR_STUB=y
@@ -296,9 +304,8 @@
 CONFIG_V4L_PLATFORM_DRIVERS=y
 CONFIG_MSM_CAMERA_V4L2=y
 CONFIG_MT9M114=y
-CONFIG_OV2720=y
-CONFIG_MSM_CAMERA_FLASH=y
 CONFIG_MSM_CAMERA_LED_TRIGGER_FLASH=y
+CONFIG_OV2720=y
 CONFIG_MSM_CAMERA_SENSOR=y
 CONFIG_MSM_ACTUATOR=y
 CONFIG_MSM_JPEG=y
@@ -330,8 +337,10 @@
 CONFIG_SND_USB_AUDIO=y
 CONFIG_SND_SOC=y
 CONFIG_SND_SOC_MSM8974=y
-CONFIG_USB=y
 CONFIG_USB_XHCI_HCD=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_ROOT_HUB_TT=y
+CONFIG_USB_EHCI_MSM_HSIC=y
 CONFIG_USB_ACM=y
 CONFIG_USB_STORAGE=y
 CONFIG_USB_STORAGE_DATAFAB=y
diff --git a/arch/arm/configs/msm8974_defconfig b/arch/arm/configs/msm8974_defconfig
index f9dbc85..ca60319 100644
--- a/arch/arm/configs/msm8974_defconfig
+++ b/arch/arm/configs/msm8974_defconfig
@@ -66,8 +66,11 @@
 CONFIG_MSM_OCMEM_LOCAL_POWER_CTRL=y
 CONFIG_MSM_OCMEM_DEBUG=y
 CONFIG_MSM_OCMEM_NONSECURE=y
+CONFIG_MSM_RTB=y
+CONFIG_MSM_RTB_SEPARATE_CPUS=y
 CONFIG_MSM_CACHE_ERP=y
 CONFIG_MSM_L1_ERR_PANIC=y
+CONFIG_MSM_L1_RECOV_ERR_PANIC=y
 CONFIG_MSM_L1_ERR_LOG=y
 CONFIG_MSM_L2_ERP_PRINT_ACCESS_ERRORS=y
 CONFIG_MSM_L2_ERP_2BIT_PANIC=y
@@ -79,6 +82,7 @@
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_SMP=y
 # CONFIG_SMP_ON_UP is not set
+CONFIG_SCHED_MC=y
 CONFIG_ARM_ARCH_TIMER=y
 CONFIG_PREEMPT=y
 CONFIG_AEABI=y
@@ -220,8 +224,9 @@
 CONFIG_GENLOCK_MISCDEVICE=y
 CONFIG_BLK_DEV_LOOP=y
 CONFIG_BLK_DEV_RAM=y
-CONFIG_TI_DRV2667=y
 CONFIG_QSEECOM=y
+CONFIG_USB_HSIC_SMSC_HUB=y
+CONFIG_TI_DRV2667=y
 CONFIG_SCSI=y
 CONFIG_SCSI_TGT=y
 CONFIG_BLK_DEV_SD=y
@@ -250,6 +255,7 @@
 CONFIG_INPUT_EVBUG=m
 CONFIG_KEYBOARD_GPIO=y
 CONFIG_INPUT_JOYSTICK=y
+CONFIG_JOYSTICK_XPAD=y
 CONFIG_INPUT_TOUCHSCREEN=y
 CONFIG_TOUCHSCREEN_ATMEL_MXT=y
 CONFIG_INPUT_MISC=y
@@ -285,6 +291,7 @@
 CONFIG_THERMAL=y
 CONFIG_THERMAL_TSENS8974=y
 CONFIG_THERMAL_QPNP=y
+CONFIG_THERMAL_QPNP_ADC_TM=y
 CONFIG_WCD9320_CODEC=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_REGULATOR_STUB=y
@@ -298,9 +305,8 @@
 CONFIG_V4L_PLATFORM_DRIVERS=y
 CONFIG_MSM_CAMERA_V4L2=y
 CONFIG_MT9M114=y
-CONFIG_OV2720=y
-CONFIG_MSM_CAMERA_FLASH=y
 CONFIG_MSM_CAMERA_LED_TRIGGER_FLASH=y
+CONFIG_OV2720=y
 CONFIG_MSM_CAMERA_SENSOR=y
 CONFIG_MSM_ACTUATOR=y
 CONFIG_MSM_JPEG=y
@@ -332,8 +338,10 @@
 CONFIG_SND_USB_AUDIO=y
 CONFIG_SND_SOC=y
 CONFIG_SND_SOC_MSM8974=y
-CONFIG_USB=y
 CONFIG_USB_XHCI_HCD=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_ROOT_HUB_TT=y
+CONFIG_USB_EHCI_MSM_HSIC=y
 CONFIG_USB_ACM=y
 CONFIG_USB_STORAGE=y
 CONFIG_USB_STORAGE_DATAFAB=y
@@ -405,7 +413,7 @@
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_LOCKUP_DETECTOR=y
 # CONFIG_DETECT_HUNG_TASK is not set
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 CONFIG_DEBUG_KMEMLEAK=y
 CONFIG_DEBUG_KMEMLEAK_DEFAULT_OFF=y
diff --git a/arch/arm/configs/msm9615_defconfig b/arch/arm/configs/msm9615_defconfig
index a052609..06ec01a 100644
--- a/arch/arm/configs/msm9615_defconfig
+++ b/arch/arm/configs/msm9615_defconfig
@@ -275,7 +275,7 @@
 CONFIG_PRINTK_TIME=y
 CONFIG_MAGIC_SYSRQ=y
 CONFIG_DEBUG_FS=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 # CONFIG_DEBUG_PREEMPT is not set
 CONFIG_DEBUG_INFO=y
diff --git a/arch/arm/configs/msm9625_defconfig b/arch/arm/configs/msm9625_defconfig
index 80f16d4..a3e7b98 100644
--- a/arch/arm/configs/msm9625_defconfig
+++ b/arch/arm/configs/msm9625_defconfig
@@ -47,8 +47,10 @@
 CONFIG_MSM_PIL=y
 CONFIG_MSM_PIL_MSS_QDSP6V5=y
 CONFIG_MSM_DIRECT_SCLK_ACCESS=y
+CONFIG_MSM_BUS_SCALING=y
 CONFIG_MSM_WATCHDOG_V2=y
 CONFIG_MSM_DLOAD_MODE=y
+CONFIG_MSM_ADSP_LOADER=m
 CONFIG_NO_HZ=y
 CONFIG_HIGH_RES_TIMERS=y
 CONFIG_ARM_ARCH_TIMER=y
@@ -71,7 +73,14 @@
 CONFIG_PACKET=y
 CONFIG_UNIX=y
 CONFIG_INET=y
+CONFIG_IP_MULTICAST=y
+CONFIG_IP_ADVANCED_ROUTER=y
+CONFIG_IP_MULTIPLE_TABLES=y
+CONFIG_IP_MROUTE=y
+CONFIG_IP_PIMSM_V2=y
 CONFIG_IPV6=y
+CONFIG_IPV6_MULTIPLE_TABLES=y
+CONFIG_IPV6_SUBTREES=y
 CONFIG_NETFILTER=y
 CONFIG_NETFILTER_DEBUG=y
 CONFIG_NETFILTER_NETLINK_QUEUE=y
@@ -83,6 +92,47 @@
 CONFIG_NF_CONNTRACK_PPTP=y
 CONFIG_NF_CONNTRACK_SIP=y
 CONFIG_NF_CONNTRACK_TFTP=y
+CONFIG_NETFILTER_XT_MARK=y
+CONFIG_NETFILTER_XT_CONNMARK=y
+CONFIG_NETFILTER_XT_MATCH_ADDRTYPE=y
+CONFIG_NETFILTER_XT_MATCH_CONNLIMIT=y
+CONFIG_NETFILTER_XT_MATCH_CONNTRACK=y
+CONFIG_NETFILTER_XT_MATCH_IPRANGE=y
+CONFIG_NETFILTER_XT_MATCH_MULTIPORT=y
+CONFIG_NETFILTER_XT_MATCH_PKTTYPE=y
+CONFIG_IP_SET=y
+CONFIG_NF_CONNTRACK_IPV4=y
+CONFIG_IP_NF_IPTABLES=y
+CONFIG_IP_NF_MATCH_AH=y
+CONFIG_IP_NF_MATCH_ECN=y
+CONFIG_IP_NF_MATCH_TTL=y
+CONFIG_IP_NF_FILTER=y
+CONFIG_IP_NF_TARGET_REJECT=y
+CONFIG_IP_NF_TARGET_REJECT_SKERR=y
+CONFIG_IP_NF_TARGET_ULOG=y
+CONFIG_NF_NAT=y
+CONFIG_IP_NF_TARGET_MASQUERADE=y
+CONFIG_IP_NF_TARGET_NATTYPE_MODULE=y
+CONFIG_IP_NF_TARGET_NETMAP=y
+CONFIG_IP_NF_TARGET_REDIRECT=y
+CONFIG_IP_NF_MANGLE=y
+CONFIG_IP_NF_TARGET_ECN=y
+CONFIG_IP_NF_TARGET_TTL=y
+CONFIG_IP_NF_RAW=y
+CONFIG_NF_CONNTRACK_IPV6=y
+CONFIG_IP6_NF_IPTABLES=y
+CONFIG_IP6_NF_MATCH_AH=y
+CONFIG_IP6_NF_MATCH_FRAG=y
+CONFIG_IP6_NF_MATCH_OPTS=y
+CONFIG_IP6_NF_MATCH_HL=y
+CONFIG_IP6_NF_MATCH_IPV6HEADER=y
+CONFIG_IP6_NF_MATCH_MH=y
+CONFIG_IP6_NF_MATCH_RT=y
+CONFIG_IP6_NF_FILTER=y
+CONFIG_IP6_NF_TARGET_REJECT=y
+CONFIG_IP6_NF_TARGET_REJECT_SKERR=y
+CONFIG_IP6_NF_MANGLE=y
+CONFIG_IP6_NF_RAW=y
 CONFIG_NET_SCHED=y
 CONFIG_NET_SCH_HTB=y
 CONFIG_NET_SCH_PRIO=y
@@ -129,7 +179,6 @@
 CONFIG_I2C=y
 CONFIG_I2C_CHARDEV=y
 CONFIG_I2C_QUP=y
-CONFIG_MSM_BUS_SCALING=y
 CONFIG_SPI=y
 CONFIG_SPI_QUP=y
 CONFIG_SPI_SPIDEV=m
@@ -145,6 +194,7 @@
 CONFIG_SENSORS_QPNP_ADC_VOLTAGE=y
 CONFIG_THERMAL=y
 CONFIG_THERMAL_TSENS8974=y
+CONFIG_WCD9320_CODEC=y
 CONFIG_REGULATOR=y
 CONFIG_REGULATOR_FIXED_VOLTAGE=y
 CONFIG_REGULATOR_QPNP=y
@@ -153,6 +203,7 @@
 CONFIG_SOUND=y
 CONFIG_SND=y
 CONFIG_SND_SOC=y
+CONFIG_SND_SOC_MDM9625=y
 CONFIG_USB_GADGET=y
 CONFIG_USB_CI13XXX_MSM=y
 CONFIG_USB_G_ANDROID=y
@@ -169,12 +220,13 @@
 CONFIG_RTC_CLASS=y
 # CONFIG_RTC_DRV_MSM is not set
 CONFIG_RTC_DRV_QPNP=y
-CONFIG_IPA=y
 CONFIG_SPS=y
 CONFIG_USB_BAM=y
 CONFIG_SPS_SUPPORT_BAMDMA=y
 CONFIG_SPS_SUPPORT_NDP_BAM=y
 CONFIG_QPNP_POWER_ON=y
+CONFIG_IPA=y
+CONFIG_MSM_QDSS=y
 CONFIG_VFAT_FS=y
 CONFIG_TMPFS=y
 CONFIG_YAFFS_FS=y
@@ -184,12 +236,12 @@
 CONFIG_NLS_ISO8859_1=y
 CONFIG_PRINTK_TIME=y
 CONFIG_MAGIC_SYSRQ=y
-CONFIG_DEBUG_FS=y
-# CONFIG_SCHED_DEBUG is not set
+CONFIG_SCHEDSTATS=y
 CONFIG_TIMER_STATS=y
 # CONFIG_DEBUG_PREEMPT is not set
 CONFIG_DEBUG_INFO=y
 CONFIG_DEBUG_MEMORY_INIT=y
+CONFIG_ENABLE_DEFAULT_TRACERS=y
 CONFIG_DYNAMIC_DEBUG=y
 CONFIG_DEBUG_USER=y
 CONFIG_KEYS=y
@@ -208,58 +260,3 @@
 # CONFIG_CRYPTO_HW is not set
 CONFIG_CRC_CCITT=y
 CONFIG_LIBCRC32C=y
-CONFIG_ENABLE_DEFAULT_TRACERS=y
-CONFIG_MSM_QDSS=y
-CONFIG_IP_MULTICAST=y
-CONFIG_IP_ADVANCED_ROUTER=y
-CONFIG_IP_MULTIPLE_TABLES=y
-CONFIG_IP_MROUTE=y
-CONFIG_IP_PIMSM_V2=y
-CONFIG_IPV6_MULTIPLE_TABLES=y
-CONFIG_IPV6_SUBTREES=y
-CONFIG_NETFILTER_XT_MARK=y
-CONFIG_NETFILTER_XT_CONNMARK=y
-CONFIG_NETFILTER_XT_MATCH_ADDRTYPE=y
-CONFIG_NETFILTER_XT_MATCH_CONNLIMIT=y
-CONFIG_NETFILTER_XT_MATCH_CONNTRACK=y
-CONFIG_NETFILTER_XT_MATCH_IPRANGE=y
-CONFIG_NETFILTER_XT_MATCH_MULTIPORT=y
-CONFIG_NETFILTER_XT_MATCH_PKTTYPE=y
-CONFIG_IP_SET=y
-CONFIG_NF_CONNTRACK_IPV4=y
-CONFIG_IP_NF_IPTABLES=y
-CONFIG_IP_NF_MATCH_AH=y
-CONFIG_IP_NF_MATCH_ECN=y
-CONFIG_IP_NF_MATCH_TTL=y
-CONFIG_IP_NF_FILTER=y
-CONFIG_IP_NF_TARGET_REJECT=y
-CONFIG_IP_NF_TARGET_REJECT_SKERR=y
-CONFIG_IP_NF_TARGET_ULOG=y
-CONFIG_NF_NAT=y
-CONFIG_IP_NF_TARGET_MASQUERADE=y
-CONFIG_IP_NF_TARGET_NETMAP=y
-CONFIG_IP_NF_TARGET_REDIRECT=y
-CONFIG_IP_NF_MANGLE=y
-CONFIG_IP_NF_TARGET_ECN=y
-CONFIG_IP_NF_TARGET_TTL=y
-CONFIG_IP_NF_RAW=y
-CONFIG_NF_CONNTRACK_IPV6=y
-CONFIG_IP6_NF_IPTABLES=y
-CONFIG_IP6_NF_MATCH_AH=y
-CONFIG_IP6_NF_MATCH_FRAG=y
-CONFIG_IP6_NF_MATCH_OPTS=y
-CONFIG_IP6_NF_MATCH_HL=y
-CONFIG_IP6_NF_MATCH_IPV6HEADER=y
-CONFIG_IP6_NF_MATCH_MH=y
-CONFIG_IP6_NF_MATCH_RT=y
-CONFIG_IP6_NF_FILTER=y
-CONFIG_IP6_NF_TARGET_REJECT=y
-CONFIG_IP6_NF_TARGET_REJECT_SKERR=y
-CONFIG_IP6_NF_MANGLE=y
-CONFIG_IP6_NF_RAW=y
-CONFIG_WCD9320_CODEC=y
-CONFIG_SOUND=y
-CONFIG_SND=y
-CONFIG_SND_SOC=y
-CONFIG_SND_SOC_MDM9625=y
-CONFIG_MSM_ADSP_LOADER=m
diff --git a/arch/arm/include/asm/mach/arch.h b/arch/arm/include/asm/mach/arch.h
index 669a626..6ea1a1e 100644
--- a/arch/arm/include/asm/mach/arch.h
+++ b/arch/arm/include/asm/mach/arch.h
@@ -14,6 +14,12 @@
 struct meminfo;
 struct sys_timer;
 struct pt_regs;
+struct smp_operations;
+#ifdef CONFIG_SMP
+#define smp_ops(ops) (&(ops))
+#else
+#define smp_ops(ops) (struct smp_operations *)NULL
+#endif
 
 struct machine_desc {
 	unsigned int		nr;		/* architecture number	*/
@@ -35,6 +41,7 @@
 	unsigned char		reserve_lp1 :1;	/* never has lp1	*/
 	unsigned char		reserve_lp2 :1;	/* never has lp2	*/
 	char			restart_mode;	/* default restart mode	*/
+	struct smp_operations  *smp;    /* SMP operations  */
 	void			(*fixup)(struct tag *, char **,
 					 struct meminfo *);
 	void			(*reserve)(void);/* reserve mem blocks	*/
diff --git a/arch/arm/include/asm/smp.h b/arch/arm/include/asm/smp.h
index 7f74b59..581f4fa 100644
--- a/arch/arm/include/asm/smp.h
+++ b/arch/arm/include/asm/smp.h
@@ -95,4 +95,37 @@
 
 extern void smp_send_all_cpu_backtrace(void);
 
+struct smp_operations {
+#ifdef CONFIG_SMP
+/*
+ * Setup the set of possible CPUs (via set_cpu_possible)
+ */
+void (*smp_init_cpus)(void);
+/*
+ * Initialize cpu_possible map, and enable coherency
+ */
+void (*smp_prepare_cpus)(unsigned int max_cpus);
+
+/*
+ * Perform platform specific initialisation of the specified CPU.
+ */
+void (*smp_secondary_init)(unsigned int cpu);
+/*
+ * Boot a secondary CPU, and assign it the specified idle task.
+ * This also gives us the initial stack to use for this CPU.
+ */
+int  (*smp_boot_secondary)(unsigned int cpu, struct task_struct *idle);
+#ifdef CONFIG_HOTPLUG_CPU
+int  (*cpu_kill)(unsigned int cpu);
+void (*cpu_die)(unsigned int cpu);
+int  (*cpu_disable)(unsigned int cpu);
+#endif
+#endif
+};
+
+/*
+ * set platform specific SMP operations
+ */
+extern void smp_set_ops(struct smp_operations *);
+
 #endif /* ifndef __ASM_ARM_SMP_H */
diff --git a/arch/arm/kernel/perf_event.c b/arch/arm/kernel/perf_event.c
index 3a52ddc..d2e2e44 100644
--- a/arch/arm/kernel/perf_event.c
+++ b/arch/arm/kernel/perf_event.c
@@ -444,7 +444,7 @@
 
 	if (plat && plat->free_pmu_irq)
 		armpmu->free_pmu_irq = plat->free_pmu_irq;
-	else if (!armpmu->request_pmu_irq)
+	else if (!armpmu->free_pmu_irq)
 		armpmu->free_pmu_irq = armpmu_generic_free_irq;
 
 	irqs = min(pmu_device->num_resources, num_possible_cpus());
diff --git a/arch/arm/kernel/setup.c b/arch/arm/kernel/setup.c
index b012f0f..4aabf0e 100644
--- a/arch/arm/kernel/setup.c
+++ b/arch/arm/kernel/setup.c
@@ -974,8 +974,10 @@
 	unflatten_device_tree();
 
 #ifdef CONFIG_SMP
-	if (is_smp())
+	if (is_smp()) {
+		smp_set_ops(mdesc->smp);
 		smp_init_cpus();
+	}
 #endif
 	reserve_crashkernel();
 
diff --git a/arch/arm/kernel/smp.c b/arch/arm/kernel/smp.c
index e46fd92..dea0f1f 100644
--- a/arch/arm/kernel/smp.c
+++ b/arch/arm/kernel/smp.c
@@ -19,7 +19,6 @@
 #include <linux/mm.h>
 #include <linux/err.h>
 #include <linux/cpu.h>
-#include <linux/smp.h>
 #include <linux/seq_file.h>
 #include <linux/irq.h>
 #include <linux/percpu.h>
@@ -27,6 +26,7 @@
 #include <linux/completion.h>
 
 #include <linux/atomic.h>
+#include <asm/smp.h>
 #include <asm/cacheflush.h>
 #include <asm/cpu.h>
 #include <asm/cputype.h>
@@ -42,6 +42,7 @@
 #include <asm/ptrace.h>
 #include <asm/localtimer.h>
 #include <asm/smp_plat.h>
+#include <asm/mach/arch.h>
 
 /*
  * as from 2.5, kernels no longer have an init_tasks structure
@@ -62,6 +63,14 @@
 
 static DECLARE_COMPLETION(cpu_running);
 
+static struct smp_operations smp_ops;
+
+void __init smp_set_ops(struct smp_operations *ops)
+{
+	if (ops)
+		smp_ops = *ops;
+};
+
 int __cpuinit __cpu_up(unsigned int cpu)
 {
 	struct cpuinfo_arm *ci = &per_cpu(cpu_data, cpu);
@@ -123,9 +132,61 @@
 	return ret;
 }
 
+/* platform specific SMP operations */
+void __attribute__((weak)) __init smp_init_cpus(void)
+{
+	if (smp_ops.smp_init_cpus)
+		smp_ops.smp_init_cpus();
+}
+
+void __attribute__((weak)) __init platform_smp_prepare_cpus(unsigned int max_cpus)
+{
+	if (smp_ops.smp_prepare_cpus)
+		smp_ops.smp_prepare_cpus(max_cpus);
+}
+
+void __attribute__((weak)) __cpuinit platform_secondary_init(unsigned int cpu)
+{
+	if (smp_ops.smp_secondary_init)
+		smp_ops.smp_secondary_init(cpu);
+}
+
+int __attribute__((weak)) __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	if (smp_ops.smp_boot_secondary)
+		return smp_ops.smp_boot_secondary(cpu, idle);
+	return -ENOSYS;
+}
+
 #ifdef CONFIG_HOTPLUG_CPU
 static void percpu_timer_stop(void);
 
+int __attribute__((weak)) platform_cpu_kill(unsigned int cpu)
+{
+	if (smp_ops.cpu_kill)
+		return smp_ops.cpu_kill(cpu);
+	return 1;
+}
+
+void __attribute__((weak)) platform_cpu_die(unsigned int cpu)
+{
+	if (smp_ops.cpu_die)
+		smp_ops.cpu_die(cpu);
+}
+
+int __attribute__((weak)) platform_cpu_disable(unsigned int cpu)
+{
+	if (smp_ops.cpu_disable)
+		return smp_ops.cpu_disable(cpu);
+
+	/*
+	* By default, allow disabling all CPUs except the first one,
+	* since this is special on a lot of platforms, e.g. because
+	* of clock tick interrupts.
+	*/
+	return cpu == 0 ? -EPERM : 0;
+}
+
 /*
  * __cpu_disable runs on the processor to be shutdown.
  */
diff --git a/arch/arm/mach-msm/Kconfig b/arch/arm/mach-msm/Kconfig
index 5e51bb8..8dd8579 100644
--- a/arch/arm/mach-msm/Kconfig
+++ b/arch/arm/mach-msm/Kconfig
@@ -167,7 +167,6 @@
 	select MSM_NATIVE_RESTART
 	select DONT_MAP_HOLE_AFTER_MEMBANK0
 	select MSM_REMOTE_SPINLOCK_SFPB
-	select ARCH_SPARSEMEM_ENABLE
 	select ARCH_HAS_HOLES_MEMORYMODEL
 	select CLEANCACHE
 	select QCACHE
@@ -184,6 +183,7 @@
 	select USE_USER_ACCESSIBLE_TIMERS
 	select ARM_USE_USER_ACCESSIBLE_TIMERS
 	select MSM_USE_USER_ACCESSIBLE_TIMERS
+	select MSM_CPU_PWRCTL
 
 config ARCH_MSM8930
 	bool "MSM8930"
@@ -208,7 +208,6 @@
 	select MSM_NATIVE_RESTART
 	select DONT_MAP_HOLE_AFTER_MEMBANK0
 	select MSM_REMOTE_SPINLOCK_SFPB
-	select ARCH_SPARSEMEM_ENABLE
 	select ARCH_HAS_HOLES_MEMORYMODEL
 	select MSM_ULTRASOUND_A
 	select MULTI_IRQ_HANDLER
@@ -221,6 +220,7 @@
 	select USE_USER_ACCESSIBLE_TIMERS
 	select ARM_USE_USER_ACCESSIBLE_TIMERS
 	select MSM_USE_USER_ACCESSIBLE_TIMERS
+	select MSM_CPU_PWRCTL
 
 config ARCH_APQ8064
 	bool "APQ8064"
@@ -254,6 +254,7 @@
 	select USE_USER_ACCESSIBLE_TIMERS
 	select ARM_USE_USER_ACCESSIBLE_TIMERS
 	select MSM_USE_USER_ACCESSIBLE_TIMERS
+	select MSM_CPU_PWRCTL
 
 config ARCH_MSM8974
 	bool "MSM8974"
@@ -285,6 +286,7 @@
 	select MSM_RPM_STATS_LOG
 	select QMI_ENCDEC
 	select DONT_MAP_HOLE_AFTER_MEMBANK0
+	select SENSORS_ADSP
 
 config ARCH_MPQ8092
 	bool "MPQ8092"
@@ -505,7 +507,7 @@
 
 config MSM_MPM_OF
 	bool "Modem Power Manager"
-	depends on CONFIG_OF
+	depends on OF
 	help
 	  MPM is a dedicated hardware resource responsible for entering and
 	  waking up from a system wide low power mode. The MPM driver tracks
@@ -1686,17 +1688,6 @@
 
 endif # CPU_FREQ_MSM
 
-config MSM_CPU_AVS
-	bool "Enable software controlled Adaptive Voltage Scaling (AVS)"
-	depends on (ARCH_MSM_SCORPION && QSD_SVS)
-	depends on ARCH_QSD8X50
-	default n
-	select MSM_AVS_HW
-	help
-	  This enables the s/w control of Adaptive Voltage Scaling feature
-	  in Qualcomm ARMv7 CPUs. It adjusts the voltage for each frequency
-	  based on feedback from three ring oscillators in the CPU.
-
 config MSM_AVS_HW
 	bool "Enable Adaptive Voltage Scaling (AVS)"
 	default n
@@ -2112,7 +2103,7 @@
 
 config MSM_BUSPM_DEV
 	tristate "MSM Bus Performance Monitor Kernel Module"
-	depends on (ARCH_MSM8X60 || ARCH_MSM8960)
+	depends on (ARCH_MSM8X60 || ARCH_MSM8960 || ARCH_MSM8974)
 	default m
 	help
 	  This kernel module is used to mmap() hardware registers for the
@@ -2265,7 +2256,7 @@
 
 config MSM_RUN_QUEUE_STATS
 	bool "Enable collection and exporting of MSM Run Queue stats to userspace"
-	depends on (MSM_SOC_REV_A || ARCH_MSM8X60 || ARCH_MSM8960)
+	default n
 	help
 	 This option enalbes statistics collection on Run Queue. A daemon
          in user mode, called MPDecision will be using this data to decide
@@ -2492,6 +2483,14 @@
 	  never allowing them to be turned OFF. Both local power
 	  management and RPM assisted power modes are supported.
 
+config SENSORS_ADSP
+	bool "Enable Sensors Driver Support for ADSP"
+	help
+	  Add support for sensors ADSP driver.
+	  This driver is used for exercising different sensors use cases,
+	  such as for lower-power OCMEM use cases, and for time syncing
+	  with ADSP clock.
+
 config MSM_RTB
 	bool "Register tracing"
 	help
@@ -2540,6 +2539,21 @@
 
 	  For production builds, you should probably say 'N' here.
 
+config MSM_L1_RECOV_ERR_PANIC
+	bool "Panic on recoverable L1 cache errors"
+	depends on MSM_CACHE_ERP && MSM_L1_ERR_PANIC
+	help
+	  Certain CPU designs may be able to automatically recover from certain
+	  kinds of L1 cache errors, even though the L1 cache itself may not
+	  support error correction. These errors should not result in any kind
+	  of corruption, but their presence is nevertheless an indication of
+	  poor system health. To cause the kernel to panic whenever a
+	  recoverable L1 cache error is detected, say 'Y' here. This may be
+	  useful as a debugging technique if general system instability is
+	  suspected.
+
+	  For production builds, you should definitely say 'N' here.
+
 config MSM_L1_ERR_LOG
 	bool "Log CPU ERP events to system memory"
 	depends on MSM_CACHE_ERP
@@ -2715,4 +2729,11 @@
 	help
 	  Use Device Control Volume as opposed to ALSA volume control.
 
+config MSM_CPU_PWRCTL
+	bool "Ensures that krait droop detectors are always off"
+	help
+	  Droop detector mechanism can adversely affect krait plls during
+	  stand alone power collapse operation. Selecting this option
+	  ensures that they are always off.
+
 endif
diff --git a/arch/arm/mach-msm/Makefile b/arch/arm/mach-msm/Makefile
index 26aa32c..ff21190 100644
--- a/arch/arm/mach-msm/Makefile
+++ b/arch/arm/mach-msm/Makefile
@@ -28,7 +28,7 @@
 ifdef CONFIG_ARCH_MSM_KRAIT
 obj-$(CONFIG_DEBUG_FS) += acpuclock-krait-debug.o
 endif
-obj-$(CONFIG_ARCH_MSM7X27) += acpuclock-7627.o clock-pll.o
+obj-$(CONFIG_ARCH_MSM7X27) += acpuclock-7627.o acpuclock-8625q.o clock-pll.o
 obj-$(CONFIG_ARCH_MSM_SCORPION) += pmu.o
 obj-$(CONFIG_ARCH_MSM_SCORPIONMP) += perf_event_msm_l2.o
 obj-$(CONFIG_ARCH_MSM_KRAIT) += msm-krait-l2-accessors.o pmu.o perf_event_msm_krait_l2.o
@@ -45,20 +45,13 @@
 	obj-$(CONFIG_MSM_SOC_REV_NONE) += acpuclock-8x50.o
 endif
 
-obj-$(CONFIG_SMP) += headsmp.o
-ifdef CONFIG_ARCH_MSM_CORTEXMP
+obj-$(CONFIG_SMP) += headsmp.o platsmp.o
 ifdef CONFIG_ARCH_MSM8625
 	obj-$(CONFIG_SMP) += platsmp-8625.o
-else
-	obj-$(CONFIG_SMP) += platsmp-8910.o
-endif
-else
-	obj-$(CONFIG_SMP) += platsmp.o
 endif
 obj-$(CONFIG_HOTPLUG_CPU) += hotplug.o
 
-obj-$(CONFIG_MSM_CPU_AVS) += avs.o
-obj-$(CONFIG_MSM_AVS_HW) += avs_hw.o
+obj-$(CONFIG_MSM_AVS_HW) += avs.o
 obj-$(CONFIG_CPU_V6) += idle-v6.o
 obj-$(CONFIG_CPU_V7) += idle-v7.o
 obj-$(CONFIG_MSM_JTAG) += jtag.o
@@ -305,6 +298,7 @@
 obj-$(CONFIG_ARCH_MPQ8092) += board-8092.o board-8092-gpiomux.o
 obj-$(CONFIG_ARCH_MSM8226) += board-8226.o board-8226-gpiomux.o
 obj-$(CONFIG_ARCH_MSM8910) += board-8910.o board-8910-gpiomux.o
+obj-$(CONFIG_ARCH_MSM8910) += clock-local2.o clock-pll.o clock-8910.o clock-rpm.o clock-voter.o
 
 obj-$(CONFIG_MACH_SAPPHIRE) += board-sapphire.o board-sapphire-gpio.o
 obj-$(CONFIG_MACH_SAPPHIRE) += board-sapphire-keypad.o board-sapphire-panel.o
@@ -343,7 +337,13 @@
 obj-$(CONFIG_MSM_BUS_SCALING) += msm_bus/
 obj-$(CONFIG_MSM_BUSPM_DEV) += msm-buspm-dev.o
 
-obj-$(CONFIG_MSM_IOMMU)		+= devices-iommu.o iommu_domains.o
+obj-$(CONFIG_MSM_IOMMU)		+= iommu_domains.o
+ifdef CONFIG_IOMMU_SUPPORT
+obj-$(CONFIG_ARCH_MSM8960)	+= devices-iommu.o
+obj-$(CONFIG_ARCH_MSM8930)	+= devices-iommu.o
+obj-$(CONFIG_ARCH_MSM8064)	+= devices-iommu.o
+endif
+
 obj-$(CONFIG_MSM_EVENT_TIMER)		+= event_timer.o
 
 ifdef CONFIG_VCM
@@ -351,6 +351,7 @@
 endif
 obj-$(CONFIG_MSM_OCMEM) += ocmem.o ocmem_allocator.o ocmem_notifier.o
 obj-$(CONFIG_MSM_OCMEM) += ocmem_sched.o ocmem_api.o ocmem_rdm.o ocmem_core.o
+obj-$(CONFIG_SENSORS_ADSP) += sensors_adsp.o
 
 obj-$(CONFIG_ARCH_MSM7X27) += gpiomux-7x27.o gpiomux-v1.o gpiomux.o
 obj-$(CONFIG_ARCH_MSM7X30) += gpiomux-7x30.o gpiomux-v1.o gpiomux.o
@@ -412,3 +413,4 @@
 obj-$(CONFIG_MEMORY_HOLE_CARVEOUT) +=  msm_mem_hole.o
 
 obj-$(CONFIG_MSM_SMCMOD) += smcmod.o
+obj-$(CONFIG_MSM_CPU_PWRCTL) +=  msm_cpu_pwrctl.o
diff --git a/arch/arm/mach-msm/acpuclock-8064.c b/arch/arm/mach-msm/acpuclock-8064.c
index 8174370..db77a34 100644
--- a/arch/arm/mach-msm/acpuclock-8064.c
+++ b/arch/arm/mach-msm/acpuclock-8064.c
@@ -505,20 +505,20 @@
 	[0][PVS_FASTER]  = {tbl_faster, sizeof(tbl_faster), 25000 },
 
 	[1][0] = { tbl_PVS0_1700MHz, sizeof(tbl_PVS0_1700MHz),     0 },
-	[1][1] = { tbl_PVS1_1700MHz, sizeof(tbl_PVS1_1700MHz),     0 },
-	[1][2] = { tbl_PVS2_1700MHz, sizeof(tbl_PVS2_1700MHz),     0 },
-	[1][3] = { tbl_PVS3_1700MHz, sizeof(tbl_PVS3_1700MHz),     0 },
-	[1][4] = { tbl_PVS4_1700MHz, sizeof(tbl_PVS4_1700MHz),     0 },
-	[1][5] = { tbl_PVS5_1700MHz, sizeof(tbl_PVS5_1700MHz),     0 },
-	[1][6] = { tbl_PVS6_1700MHz, sizeof(tbl_PVS6_1700MHz),     0 },
+	[1][1] = { tbl_PVS1_1700MHz, sizeof(tbl_PVS1_1700MHz),     25000 },
+	[1][2] = { tbl_PVS2_1700MHz, sizeof(tbl_PVS2_1700MHz),     25000 },
+	[1][3] = { tbl_PVS3_1700MHz, sizeof(tbl_PVS3_1700MHz),     25000 },
+	[1][4] = { tbl_PVS4_1700MHz, sizeof(tbl_PVS4_1700MHz),     25000 },
+	[1][5] = { tbl_PVS5_1700MHz, sizeof(tbl_PVS5_1700MHz),     25000 },
+	[1][6] = { tbl_PVS6_1700MHz, sizeof(tbl_PVS6_1700MHz),     25000 },
 
 	[2][0] = { tbl_PVS0_2000MHz, sizeof(tbl_PVS0_2000MHz),     0 },
-	[2][1] = { tbl_PVS1_2000MHz, sizeof(tbl_PVS1_2000MHz),     0 },
-	[2][2] = { tbl_PVS2_2000MHz, sizeof(tbl_PVS2_2000MHz),     0 },
-	[2][3] = { tbl_PVS3_2000MHz, sizeof(tbl_PVS3_2000MHz),     0 },
-	[2][4] = { tbl_PVS4_2000MHz, sizeof(tbl_PVS4_2000MHz),     0 },
-	[2][5] = { tbl_PVS5_2000MHz, sizeof(tbl_PVS5_2000MHz),     0 },
-	[2][6] = { tbl_PVS6_2000MHz, sizeof(tbl_PVS6_2000MHz),     0 },
+	[2][1] = { tbl_PVS1_2000MHz, sizeof(tbl_PVS1_2000MHz),     25000 },
+	[2][2] = { tbl_PVS2_2000MHz, sizeof(tbl_PVS2_2000MHz),     25000 },
+	[2][3] = { tbl_PVS3_2000MHz, sizeof(tbl_PVS3_2000MHz),     25000 },
+	[2][4] = { tbl_PVS4_2000MHz, sizeof(tbl_PVS4_2000MHz),     25000 },
+	[2][5] = { tbl_PVS5_2000MHz, sizeof(tbl_PVS5_2000MHz),     25000 },
+	[2][6] = { tbl_PVS6_2000MHz, sizeof(tbl_PVS6_2000MHz),     25000 },
 };
 
 static struct acpuclk_krait_params acpuclk_8064_params __initdata = {
diff --git a/arch/arm/mach-msm/acpuclock-8625q.c b/arch/arm/mach-msm/acpuclock-8625q.c
new file mode 100644
index 0000000..0a6dfbe
--- /dev/null
+++ b/arch/arm/mach-msm/acpuclock-8625q.c
@@ -0,0 +1,770 @@
+/*
+ * Copyright (C) 2007 Google, Inc.
+ * Copyright (c) 2007-2012, Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/cpufreq.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <linux/sort.h>
+#include <linux/regulator/consumer.h>
+#include <linux/smp.h>
+
+#include <mach/board.h>
+#include <mach/msm_iomap.h>
+#include <mach/clk-provider.h>
+
+#include <asm/cpu.h>
+
+#include "acpuclock.h"
+#include "acpuclock-8625q.h"
+
+#define A11S_CLK_CNTL_ADDR	(MSM_CSR_BASE + 0x100)
+#define A11S_CLK_SEL_ADDR	(MSM_CSR_BASE + 0x104)
+
+#define PLL4_L_VAL_ADDR		(MSM_CLK_CTL_BASE + 0x378)
+#define PLL4_M_VAL_ADDR		(MSM_CLK_CTL_BASE + 0x37C)
+#define PLL4_N_VAL_ADDR		(MSM_CLK_CTL_BASE + 0x380)
+
+#define POWER_COLLAPSE_KHZ 19200
+
+/* Max CPU frequency allowed by hardware while in standby waiting for an irq. */
+#define MAX_WAIT_FOR_IRQ_KHZ 128000
+
+/**
+ * enum - For acpuclock PLL IDs
+ */
+enum {
+	ACPU_PLL_0	= 0,
+	ACPU_PLL_1,
+	ACPU_PLL_2,
+	ACPU_PLL_3,
+	ACPU_PLL_4,
+	ACPU_PLL_TCXO,
+	ACPU_PLL_END,
+};
+
+struct acpu_clk_src {
+	struct clk *clk;
+	const char *name;
+};
+
+struct pll_config {
+	unsigned int l;
+	unsigned int m;
+	unsigned int n;
+};
+
+static struct acpu_clk_src pll_clk[ACPU_PLL_END] = {
+	[ACPU_PLL_0] = { .name = "pll0_clk" },
+	[ACPU_PLL_1] = { .name = "pll1_clk" },
+	[ACPU_PLL_2] = { .name = "pll2_clk" },
+	[ACPU_PLL_4] = { .name = "pll4_clk" },
+};
+
+static struct pll_config pll4_cfg_tbl[] = {
+	[0] = {  36, 1, 2 }, /*  700.8 MHz */
+	[1] = {  52, 1, 2 }, /* 1008 MHz */
+	[2] = {  63, 0, 1 }, /* 1209.6 MHz */
+	[3] = {  73, 0, 1 }, /* 1401.6 MHz */
+};
+
+struct clock_state {
+	struct clkctl_acpu_speed	*current_speed;
+	struct mutex			lock;
+	uint32_t			max_speed_delta_khz;
+	struct clk			*ebi1_clk;
+	struct regulator		*vreg_cpu;
+};
+
+struct clkctl_acpu_speed {
+	unsigned int	use_for_scaling;
+	unsigned int	a11clk_khz;
+	int		pll;
+	unsigned int	a11clk_src_sel;
+	unsigned int	a11clk_src_div;
+	unsigned int	ahbclk_khz;
+	unsigned int	ahbclk_div;
+	int		vdd;
+	unsigned int	axiclk_khz;
+	struct pll_config *pll_rate;
+	unsigned long   lpj;
+};
+
+static struct clock_state drv_state = { 0 };
+
+/* PVS MAX Voltage in uV as per frequencies*/
+
+# define MAX_14GHZ_VOLTAGE 1350000
+# define MAX_12GHZ_VOLTAGE 1275000
+# define MAX_1GHZ_VOLTAGE 1175000
+# define MAX_NOMINAL_VOLTAGE 1150000
+
+/* PVS deltas as per formula*/
+# define DELTA_LEVEL_1_UV 0
+# define DELTA_LEVEL_2_UV 75000
+# define DELTA_LEVEL_3_UV 150000
+
+
+static struct clkctl_acpu_speed acpu_freq_tbl_cmn[] = {
+	{ 0, 19200, ACPU_PLL_TCXO, 0, 0, 2400, 3, 0, 30720 },
+	{ 1, 245760, ACPU_PLL_1, 1, 0, 30720, 3, MAX_NOMINAL_VOLTAGE, 61440 },
+	{ 1, 320000, ACPU_PLL_0, 4, 2, 40000, 3, MAX_NOMINAL_VOLTAGE, 122880 },
+	{ 1, 480000, ACPU_PLL_0, 4, 1, 60000, 3, MAX_NOMINAL_VOLTAGE, 122880 },
+	{ 0, 600000, ACPU_PLL_2, 2, 1, 75000, 3, 0, 160000 },
+	{ 1, 700800, ACPU_PLL_4, 6, 0, 87500, 3, MAX_NOMINAL_VOLTAGE, 160000,
+						&pll4_cfg_tbl[0]},
+	{ 1, 1008000, ACPU_PLL_4, 6, 0, 126000, 3, MAX_1GHZ_VOLTAGE, 200000,
+						&pll4_cfg_tbl[1]},
+};
+
+static struct clkctl_acpu_speed acpu_freq_tbl_1209[] = {
+	{ 1, 1209600, ACPU_PLL_4, 6, 0, 151200, 3, MAX_12GHZ_VOLTAGE, 200000,
+						&pll4_cfg_tbl[2]},
+};
+
+static struct clkctl_acpu_speed acpu_freq_tbl_1401[] = {
+	{ 1, 1401600, ACPU_PLL_4, 6, 0, 175000, 3, MAX_14GHZ_VOLTAGE, 200000,
+						&pll4_cfg_tbl[3]},
+};
+
+/* Entry corresponding to CDMA build*/
+static struct clkctl_acpu_speed acpu_freq_tbl_196608[] = {
+	{ 1, 196608, ACPU_PLL_1, 1, 0,  65536, 2, MAX_NOMINAL_VOLTAGE, 98304 },
+};
+
+static struct clkctl_acpu_speed acpu_freq_tbl_null[] = {
+	{ 0 },
+};
+
+static struct clkctl_acpu_speed acpu_freq_tbl[ARRAY_SIZE(acpu_freq_tbl_cmn)
+	+ ARRAY_SIZE(acpu_freq_tbl_1209)
+	+ ARRAY_SIZE(acpu_freq_tbl_1401)
+	+ ARRAY_SIZE(acpu_freq_tbl_null)];
+
+/* Switch to this when reprogramming PLL4 */
+static struct clkctl_acpu_speed *backup_s;
+
+#ifdef CONFIG_CPU_FREQ_MSM
+static struct cpufreq_frequency_table freq_table[NR_CPUS][20];
+
+static void __devinit cpufreq_table_init(void)
+{
+	int cpu;
+	for_each_possible_cpu(cpu) {
+		unsigned int i, freq_cnt = 0;
+
+		/* Construct the freq_table table from acpu_freq_tbl since
+		 * the freq_table values need to match frequencies specified
+		 * in acpu_freq_tbl and acpu_freq_tbl needs to be fixed up
+		 * during init.
+		 */
+		for (i = 0; acpu_freq_tbl[i].a11clk_khz != 0
+				&& freq_cnt < ARRAY_SIZE(*freq_table)-1; i++) {
+			if (acpu_freq_tbl[i].use_for_scaling) {
+				freq_table[cpu][freq_cnt].index = freq_cnt;
+				freq_table[cpu][freq_cnt].frequency
+					= acpu_freq_tbl[i].a11clk_khz;
+				freq_cnt++;
+			}
+		}
+
+		/* freq_table not big enough to store all usable freqs. */
+		BUG_ON(acpu_freq_tbl[i].a11clk_khz != 0);
+
+		freq_table[cpu][freq_cnt].index = freq_cnt;
+		freq_table[cpu][freq_cnt].frequency = CPUFREQ_TABLE_END;
+		/* Register table with CPUFreq. */
+		cpufreq_frequency_table_get_attr(freq_table[cpu], cpu);
+		pr_info("CPU%d: %d scaling frequencies supported.\n",
+			cpu, freq_cnt);
+	}
+}
+#else
+static void __devinit cpufreq_table_init(void) { }
+#endif
+
+static void update_jiffies(int cpu, unsigned long loops)
+{
+#ifdef CONFIG_SMP
+	for_each_possible_cpu(cpu) {
+		per_cpu(cpu_data, cpu).loops_per_jiffy =
+						loops;
+	}
+#endif
+	/* Adjust the global one */
+	loops_per_jiffy = loops;
+}
+
+/* Assumes PLL4 is off and the acpuclock isn't sourced from PLL4 */
+static void acpuclk_config_pll4(struct pll_config *pll)
+{
+	/*
+	 * Make sure write to disable PLL_4 has completed
+	 * before reconfiguring that PLL.
+	*/
+	mb();
+	writel_relaxed(pll->l, PLL4_L_VAL_ADDR);
+	writel_relaxed(pll->m, PLL4_M_VAL_ADDR);
+	writel_relaxed(pll->n, PLL4_N_VAL_ADDR);
+	/* Make sure PLL is programmed before returning. */
+	mb();
+}
+
+/* Set proper dividers for the given clock speed. */
+static void acpuclk_set_div(const struct clkctl_acpu_speed *hunt_s)
+{
+	uint32_t reg_clkctl, reg_clksel, clk_div, src_sel;
+
+	reg_clksel = readl_relaxed(A11S_CLK_SEL_ADDR);
+
+	/* AHB_CLK_DIV */
+	clk_div = (reg_clksel >> 1) & 0x03;
+	/* CLK_SEL_SRC1NO */
+	src_sel = reg_clksel & 1;
+
+	/*
+	 * If the new clock divider is higher than the previous, then
+	 * program the divider before switching the clock
+	 */
+	if (hunt_s->ahbclk_div > clk_div) {
+		reg_clksel &= ~(0x3 << 1);
+		reg_clksel |= (hunt_s->ahbclk_div << 1);
+		writel_relaxed(reg_clksel, A11S_CLK_SEL_ADDR);
+	}
+
+	/* Program clock source and divider */
+	reg_clkctl = readl_relaxed(A11S_CLK_CNTL_ADDR);
+	reg_clkctl &= ~(0xFF << (8 * src_sel));
+	reg_clkctl |= hunt_s->a11clk_src_sel << (4 + 8 * src_sel);
+	reg_clkctl |= hunt_s->a11clk_src_div << (0 + 8 * src_sel);
+	writel_relaxed(reg_clkctl, A11S_CLK_CNTL_ADDR);
+
+	/* Program clock source selection */
+	reg_clksel ^= 1;
+	writel_relaxed(reg_clksel, A11S_CLK_SEL_ADDR);
+
+	/* Wait for the clock switch to complete */
+	mb();
+	udelay(50);
+
+	/*
+	 * If the new clock divider is lower than the previous, then
+	 * program the divider after switching the clock
+	 */
+	if (hunt_s->ahbclk_div < clk_div) {
+		reg_clksel &= ~(0x3 << 1);
+		reg_clksel |= (hunt_s->ahbclk_div << 1);
+		writel_relaxed(reg_clksel, A11S_CLK_SEL_ADDR);
+	}
+}
+
+static int acpuclk_set_vdd_level(int vdd)
+{
+	int rc;
+
+	rc = regulator_set_voltage(drv_state.vreg_cpu, vdd, vdd);
+	if (rc) {
+		pr_err("failed to set vdd=%d uV\n", vdd);
+		return rc;
+	}
+
+	return 0;
+}
+
+static int acpuclk_8625q_set_rate(int cpu, unsigned long rate,
+					enum setrate_reason reason)
+{
+	uint32_t reg_clkctl;
+	struct clkctl_acpu_speed *cur_s, *tgt_s, *strt_s;
+	int res, rc = 0;
+	unsigned int plls_enabled = 0, pll;
+	int delta;
+
+
+	if (reason == SETRATE_CPUFREQ)
+		mutex_lock(&drv_state.lock);
+
+	strt_s = cur_s = drv_state.current_speed;
+
+	WARN_ONCE(cur_s == NULL, "%s: not initialized\n", __func__);
+	if (cur_s == NULL) {
+		rc = -ENOENT;
+		goto out;
+	}
+
+	cur_s->vdd = regulator_get_voltage(drv_state.vreg_cpu);
+	if (cur_s->vdd <= 0)
+		goto out;
+
+	pr_debug("current freq=%dKhz vdd=%duV\n",
+			cur_s->a11clk_khz, cur_s->vdd);
+
+	if (rate == cur_s->a11clk_khz)
+		goto out;
+
+	for (tgt_s = acpu_freq_tbl; tgt_s->a11clk_khz != 0; tgt_s++) {
+		if (tgt_s->a11clk_khz == rate)
+			break;
+	}
+
+	if (tgt_s->a11clk_khz == 0) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	/* Choose the highest speed at or below 'rate' with same PLL. */
+	if (reason != SETRATE_CPUFREQ
+		&& tgt_s->a11clk_khz < cur_s->a11clk_khz) {
+		while (tgt_s->pll != ACPU_PLL_TCXO &&
+					tgt_s->pll != cur_s->pll) {
+			pr_debug("Intermediate frequency changes: %u\n",
+					tgt_s->a11clk_khz);
+			tgt_s--;
+		}
+	}
+
+	if (strt_s->pll != ACPU_PLL_TCXO)
+		plls_enabled |= 1 << strt_s->pll;
+
+	/* Need to do this when coming out of power collapse since some modem
+	 * firmwares reset the VDD when the application processor enters power
+	 * collapse.
+	 */
+	if (reason == SETRATE_CPUFREQ || reason == SETRATE_PC) {
+		/* Increase VDD if needed. */
+		if (tgt_s->vdd > cur_s->vdd) {
+			rc = acpuclk_set_vdd_level(tgt_s->vdd);
+			if (rc < 0) {
+				pr_err("Unable to switch ACPU vdd (%d)\n", rc);
+				goto out;
+			}
+			pr_debug("Increased Vdd to %duV\n", tgt_s->vdd);
+		}
+	}
+
+	/* Set wait states for CPU inbetween frequency changes */
+	reg_clkctl = readl_relaxed(A11S_CLK_CNTL_ADDR);
+	reg_clkctl |= (100 << 16); /* set WT_ST_CNT */
+	writel_relaxed(reg_clkctl, A11S_CLK_CNTL_ADDR);
+
+	pr_debug("Switching from ACPU rate %u KHz -> %u KHz\n",
+			strt_s->a11clk_khz, tgt_s->a11clk_khz);
+
+	delta = abs((int)(strt_s->a11clk_khz - tgt_s->a11clk_khz));
+
+	if (tgt_s->pll == ACPU_PLL_4) {
+		if (strt_s->pll == ACPU_PLL_4 ||
+				delta > drv_state.max_speed_delta_khz) {
+			/*
+			 * Enable the backup PLL if required
+			 * and switch to it.
+			 */
+			clk_enable(pll_clk[backup_s->pll].clk);
+			acpuclk_set_div(backup_s);
+			update_jiffies(cpu, backup_s->lpj);
+		}
+		/* Make sure PLL4 is off before reprogramming */
+		if ((plls_enabled & (1 << tgt_s->pll))) {
+			clk_disable(pll_clk[tgt_s->pll].clk);
+			plls_enabled &= ~(1 << tgt_s->pll);
+		}
+		acpuclk_config_pll4(tgt_s->pll_rate);
+		pll_clk[tgt_s->pll].clk->rate = tgt_s->a11clk_khz*1000;
+
+	} else if (strt_s->pll == ACPU_PLL_4) {
+		if (delta > drv_state.max_speed_delta_khz) {
+			/*
+			 * Enable the bcackup PLL if required
+			 * and switch to it.
+			 */
+			clk_enable(pll_clk[backup_s->pll].clk);
+			acpuclk_set_div(backup_s);
+			update_jiffies(cpu, backup_s->lpj);
+		}
+	}
+
+	if ((tgt_s->pll != ACPU_PLL_TCXO) &&
+			!(plls_enabled & (1 << tgt_s->pll))) {
+		rc = clk_enable(pll_clk[tgt_s->pll].clk);
+		if (rc < 0) {
+			pr_err("PLL%d enable failed (%d)\n",
+					tgt_s->pll, rc);
+			goto out;
+		}
+		plls_enabled |= 1 << tgt_s->pll;
+	}
+	acpuclk_set_div(tgt_s);
+	drv_state.current_speed = tgt_s;
+	pr_debug("The new clock speed is %u\n", tgt_s->a11clk_khz);
+	/* Re-adjust lpj for the new clock speed. */
+	update_jiffies(cpu, tgt_s->lpj);
+
+	/* Disable the backup PLL */
+	if ((delta > drv_state.max_speed_delta_khz)
+			|| (strt_s->pll == ACPU_PLL_4 &&
+				tgt_s->pll == ACPU_PLL_4))
+		clk_disable(pll_clk[backup_s->pll].clk);
+
+	/* Nothing else to do for SWFI. */
+	if (reason == SETRATE_SWFI)
+		goto out;
+
+	/* Change the AXI bus frequency if we can. */
+	if (reason != SETRATE_PC &&
+		strt_s->axiclk_khz != tgt_s->axiclk_khz) {
+		res = clk_set_rate(drv_state.ebi1_clk,
+				tgt_s->axiclk_khz * 1000);
+		pr_debug("AXI bus set freq %d\n",
+				tgt_s->axiclk_khz * 1000);
+		if (res < 0)
+			pr_warning("Setting AXI min rate failed (%d)\n", res);
+	}
+
+	/* Disable PLLs we are not using anymore. */
+	if (tgt_s->pll != ACPU_PLL_TCXO)
+		plls_enabled &= ~(1 << tgt_s->pll);
+	for (pll = ACPU_PLL_0; pll < ACPU_PLL_END; pll++)
+		if (plls_enabled & (1 << pll))
+			clk_disable(pll_clk[pll].clk);
+
+	/* Nothing else to do for power collapse. */
+	if (reason == SETRATE_PC)
+		goto out;
+
+	/* Drop VDD level if we can. */
+	if (tgt_s->vdd < strt_s->vdd) {
+		res = acpuclk_set_vdd_level(tgt_s->vdd);
+		if (res < 0)
+			pr_warning("Unable to drop ACPU vdd (%d)\n", res);
+		pr_debug("Decreased Vdd to %duV\n", tgt_s->vdd);
+	}
+
+	pr_debug("ACPU speed change complete\n");
+out:
+	if (reason == SETRATE_CPUFREQ)
+		mutex_unlock(&drv_state.lock);
+
+	return rc;
+}
+
+static int __devinit acpuclk_hw_init(void)
+{
+	struct clkctl_acpu_speed *speed;
+	uint32_t div, sel, reg_clksel;
+	int res;
+
+	/*
+	 * Prepare all the PLLs because we enable/disable them
+	 * from atomic context and can't always ensure they're
+	 * all prepared in non-atomic context. Same goes for
+	 * ebi1_acpu_clk.
+	 */
+	BUG_ON(clk_prepare(pll_clk[ACPU_PLL_0].clk));
+	BUG_ON(clk_prepare(pll_clk[ACPU_PLL_1].clk));
+	BUG_ON(clk_prepare(pll_clk[ACPU_PLL_2].clk));
+	BUG_ON(clk_prepare(pll_clk[ACPU_PLL_4].clk));
+	BUG_ON(clk_prepare(drv_state.ebi1_clk));
+
+	/*
+	 * Determine the rate of ACPU clock
+	 */
+
+	if (!(readl_relaxed(A11S_CLK_SEL_ADDR) & 0x01)) { /* CLK_SEL_SRC1N0 */
+		/* CLK_SRC0_SEL */
+		sel = (readl_relaxed(A11S_CLK_CNTL_ADDR) >> 12) & 0x7;
+		/* CLK_SRC0_DIV */
+		div = (readl_relaxed(A11S_CLK_CNTL_ADDR) >> 8) & 0x0f;
+	} else {
+		/* CLK_SRC1_SEL */
+		sel = (readl_relaxed(A11S_CLK_CNTL_ADDR) >> 4) & 0x07;
+		/* CLK_SRC1_DIV */
+		div = readl_relaxed(A11S_CLK_CNTL_ADDR) & 0x0f;
+	}
+
+	for (speed = acpu_freq_tbl; speed->a11clk_khz != 0; speed++) {
+		if (speed->a11clk_src_sel == sel
+			&& (speed->a11clk_src_div == div))
+			break;
+	}
+	if (speed->a11clk_khz == 0) {
+		pr_err("Error - ACPU clock reports invalid speed\n");
+		return -EINVAL;
+	}
+
+	drv_state.current_speed = speed;
+	if (speed->pll != ACPU_PLL_TCXO) {
+		if (clk_enable(pll_clk[speed->pll].clk)) {
+			pr_warning("Failed to vote for boot PLL\n");
+			return -ENODEV;
+		}
+	}
+
+	reg_clksel = readl_relaxed(A11S_CLK_SEL_ADDR);
+	reg_clksel &= ~(0x3 << 14);
+	reg_clksel |= (0x1 << 14);
+	writel_relaxed(reg_clksel, A11S_CLK_SEL_ADDR);
+
+	res = clk_set_rate(drv_state.ebi1_clk, speed->axiclk_khz * 1000);
+	if (res < 0) {
+		pr_warning("Setting AXI min rate failed (%d)\n", res);
+		return -ENODEV;
+	}
+	res = clk_enable(drv_state.ebi1_clk);
+	if (res < 0) {
+		pr_warning("Enabling AXI clock failed (%d)\n", res);
+		return -ENODEV;
+	}
+
+	drv_state.vreg_cpu = regulator_get(NULL, "vddx_cx");
+	if (IS_ERR(drv_state.vreg_cpu)) {
+		res = PTR_ERR(drv_state.vreg_cpu);
+		pr_err("could not get regulator: %d\n", res);
+	}
+
+	pr_info("ACPU running at %d KHz\n", speed->a11clk_khz);
+	return 0;
+}
+
+static unsigned long acpuclk_8625q_get_rate(int cpu)
+{
+	WARN_ONCE(drv_state.current_speed == NULL,
+			"%s: not initialized\n", __func__);
+	if (drv_state.current_speed)
+		return drv_state.current_speed->a11clk_khz;
+	else
+		return 0;
+}
+
+#define MHZ 1000000
+
+static void __devinit select_freq_plan(unsigned int pvs_voltage,
+					unsigned int nominal_vol_uv,
+					unsigned int default_vol_uv)
+{
+	unsigned long pll_mhz[ACPU_PLL_END];
+	int i;
+	int size;
+	int delta[3] = {DELTA_LEVEL_1_UV, DELTA_LEVEL_2_UV, DELTA_LEVEL_3_UV};
+	struct clkctl_acpu_speed *tbl;
+
+	/* Get PLL clocks */
+	for (i = 0; i < ACPU_PLL_END; i++) {
+		if (pll_clk[i].name) {
+			pll_clk[i].clk = clk_get_sys("acpu", pll_clk[i].name);
+			if (IS_ERR(pll_clk[i].clk)) {
+				pll_mhz[i] = 0;
+				continue;
+			}
+			/* Get PLL's Rate */
+			pll_mhz[i] = clk_get_rate(pll_clk[i].clk)/MHZ;
+		}
+	}
+
+	memcpy(acpu_freq_tbl, acpu_freq_tbl_cmn, sizeof(acpu_freq_tbl_cmn));
+	size = ARRAY_SIZE(acpu_freq_tbl_cmn);
+
+	i = 0;		/* needed if we have a 1Ghz part */
+	/* select if it is a 1.2Ghz part */
+	if (pll_mhz[ACPU_PLL_4] == 1209) {
+		memcpy(acpu_freq_tbl + size, acpu_freq_tbl_1209,
+						sizeof(acpu_freq_tbl_1209));
+		size += sizeof(acpu_freq_tbl_1209);
+		i = 1;		/* set the delta index */
+	}
+	/* select if it is a 1.4Ghz part */
+	if (pll_mhz[ACPU_PLL_4] == 1401) {
+		memcpy(acpu_freq_tbl + size, acpu_freq_tbl_1209,
+						sizeof(acpu_freq_tbl_1209));
+		size += ARRAY_SIZE(acpu_freq_tbl_1209);
+		memcpy(acpu_freq_tbl + size, acpu_freq_tbl_1401,
+						sizeof(acpu_freq_tbl_1401));
+		size += ARRAY_SIZE(acpu_freq_tbl_1401);
+		i = 2;		/* set the delta index */
+	}
+
+	memcpy(acpu_freq_tbl + size, acpu_freq_tbl_null,
+						sizeof(acpu_freq_tbl_null));
+	size += sizeof(acpu_freq_tbl_null);
+
+	/* Alter the freq value in freq_tbl if it is a CDMA build*/
+	if (pll_mhz[ACPU_PLL_1] == 196) {
+
+		for (tbl = acpu_freq_tbl; tbl->a11clk_khz; tbl++) {
+			if (tbl->a11clk_khz == 245760 &&
+					tbl->pll == ACPU_PLL_1) {
+				pr_debug("Upgrading pll1 freq to 196  Mhz\n");
+				memcpy(tbl, acpu_freq_tbl_196608,
+						sizeof(acpu_freq_tbl_196608));
+				break;
+			}
+		}
+	}
+
+	/*
+	 *PVS Voltage calculation  formula
+	 *1.4 Ghz device
+	 *1.4 Ghz: Max(PVS_voltage,1.35V)
+	 *1.2 Ghz: Max(PVS_volatge - 75mV,1.275V)
+	 *1.0 Ghz: Max(PVS_voltage - 150mV, 1.175V)
+	 *1.2 Ghz device
+	 *1.2 Ghz: Max(PVS_voltage,1.275V)
+	 *1.0 Ghz: Max(PVS_volatge - 75mV,1.175V)
+	 *Nominal Mode: 1.15V
+	*/
+	for (tbl = acpu_freq_tbl; tbl->a11clk_khz; tbl++) {
+		if (tbl->a11clk_khz >= 1008000) {
+			if (tbl->a11clk_khz == 1209600)
+				tbl->vdd = default_vol_uv;
+			/*
+			 * Change voltage as per PVS formula,
+			 * i is initialized above with 2 or 1
+			 * depending upon whether it is a 1.4Ghz
+			 * or 1.2Ghz, so, we get the proper value
+			 * from delta[i] which is to be deducted
+			 * from PVS voltage.
+			*/
+
+			tbl->vdd = max((int)(pvs_voltage - delta[i]), tbl->vdd);
+			i--;
+		} else if (tbl->a11clk_khz != 600000
+					&& tbl->a11clk_khz != 19200)
+			tbl->vdd = nominal_vol_uv;
+	}
+
+
+	/* find the backup PLL entry from the table  */
+	for (tbl = acpu_freq_tbl; tbl->a11clk_khz; tbl++) {
+		if (tbl->pll == ACPU_PLL_2 &&
+				tbl->a11clk_src_div == 1) {
+			backup_s = tbl;
+			break;
+		}
+	}
+
+	BUG_ON(!backup_s);
+
+}
+
+/*
+ * Hardware requires the CPU to be dropped to less than MAX_WAIT_FOR_IRQ_KHZ
+ * before entering a wait for irq low-power mode. Find a suitable rate.
+ */
+static unsigned long __devinit find_wait_for_irq_khz(void)
+{
+	unsigned long found_khz = 0;
+	int i;
+
+	for (i = 0; acpu_freq_tbl[i].a11clk_khz &&
+		acpu_freq_tbl[i].a11clk_khz <= MAX_WAIT_FOR_IRQ_KHZ; i++)
+		found_khz = acpu_freq_tbl[i].a11clk_khz;
+
+	return found_khz;
+}
+
+static void __devinit lpj_init(void)
+{
+	int i = 0, cpu;
+	const struct clkctl_acpu_speed *base_clk = drv_state.current_speed;
+	unsigned long loops;
+
+	for_each_possible_cpu(cpu) {
+#ifdef CONFIG_SMP
+		loops = per_cpu(cpu_data, cpu).loops_per_jiffy;
+#else
+		loops = loops_per_jiffy;
+#endif
+		for (i = 0; acpu_freq_tbl[i].a11clk_khz; i++) {
+			acpu_freq_tbl[i].lpj = cpufreq_scale(
+				loops,
+				base_clk->a11clk_khz,
+				acpu_freq_tbl[i].a11clk_khz);
+		}
+
+	}
+
+}
+
+static struct acpuclk_data acpuclk_8625q_data = {
+	.set_rate = acpuclk_8625q_set_rate,
+	.get_rate = acpuclk_8625q_get_rate,
+	.power_collapse_khz = POWER_COLLAPSE_KHZ,
+	.switch_time_us = 50,
+};
+
+static void __devinit print_acpu_freq_tbl(void)
+{
+	struct clkctl_acpu_speed *t;
+	int i;
+
+	pr_info("Id CPU-KHz PLL DIV AHB-KHz ADIV AXI-KHz Vdd\n");
+
+	t = &acpu_freq_tbl[0];
+	for (i = 0; t->a11clk_khz != 0; i++) {
+		pr_info("%2d %7d %3d %3d %7d %4d %7d %3d\n",
+			i, t->a11clk_khz, t->pll, t->a11clk_src_div + 1,
+			t->ahbclk_khz, t->ahbclk_div + 1, t->axiclk_khz,
+			t->vdd);
+		t++;
+	}
+}
+
+static int __devinit acpuclk_8625q_probe(struct platform_device *pdev)
+{
+	const struct acpuclk_pdata_8625q *pdata = pdev->dev.platform_data;
+	unsigned int pvs_voltage = pdata->pvs_voltage_uv;
+	unsigned int nom_vol_uv = pdata->nominal_voltage;
+	unsigned int default_vol_uv = pdata->default_turbo_voltage;
+
+	drv_state.max_speed_delta_khz = pdata->acpu_clk_data->
+						max_speed_delta_khz;
+
+	drv_state.ebi1_clk = clk_get(NULL, "ebi1_acpu_clk");
+	BUG_ON(IS_ERR(drv_state.ebi1_clk));
+
+	mutex_init(&drv_state.lock);
+	select_freq_plan(pvs_voltage, nom_vol_uv, default_vol_uv);
+	acpuclk_8625q_data.wait_for_irq_khz = find_wait_for_irq_khz();
+
+	if (acpuclk_hw_init() < 0)
+		pr_err("acpuclk_hw_init not successful.\n");
+
+	print_acpu_freq_tbl();
+	lpj_init();
+	acpuclk_register(&acpuclk_8625q_data);
+
+	cpufreq_table_init();
+
+	return 0;
+}
+
+static struct platform_driver acpuclk_8625q_driver = {
+	.probe = acpuclk_8625q_probe,
+	.driver = {
+		.name = "acpuclock-8625q",
+		.owner = THIS_MODULE,
+	},
+};
+
+static int __init acpuclk_8625q_init(void)
+{
+
+	return platform_driver_register(&acpuclk_8625q_driver);
+}
+postcore_initcall(acpuclk_8625q_init);
diff --git a/arch/arm/mach-msm/acpuclock-8625q.h b/arch/arm/mach-msm/acpuclock-8625q.h
new file mode 100644
index 0000000..c91e3bd
--- /dev/null
+++ b/arch/arm/mach-msm/acpuclock-8625q.h
@@ -0,0 +1,30 @@
+/*
+ * MSM architecture CPU clock driver header
+ *
+ * Copyright (c) 2012, Linux Foundation. All rights reserved.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __ARCH_ARM_MACH_MSM_ACPUCLOCK_8625Q_H
+#define __ARCH_ARM_MACH_MSM_ACPUCLOCK_8625Q_H
+
+# include "acpuclock.h"
+/**
+ * struct acpuclk_pdata_8625q - Platform data for acpuclk
+ */
+struct acpuclk_pdata_8625q {
+	struct acpuclk_pdata *acpu_clk_data;
+	unsigned int pvs_voltage_uv;
+	unsigned int nominal_voltage;
+	unsigned int default_turbo_voltage;
+};
+
+#endif /* __ARCH_ARM_MACH_MSM_ACPUCLOCK_8625Q_H */
diff --git a/arch/arm/mach-msm/acpuclock-8930ab.c b/arch/arm/mach-msm/acpuclock-8930ab.c
index 96029b4..5003862 100644
--- a/arch/arm/mach-msm/acpuclock-8930ab.c
+++ b/arch/arm/mach-msm/acpuclock-8930ab.c
@@ -243,9 +243,9 @@
 
 /* TODO: Update boost voltage once the pvs data is available */
 static struct pvs_table pvs_tables[NUM_SPEED_BINS][NUM_PVS] __initdata = {
-[0][PVS_SLOW]    = { acpu_freq_tbl_slow, sizeof(acpu_freq_tbl_slow),     0 },
-[0][PVS_NOMINAL] = { acpu_freq_tbl_nom,  sizeof(acpu_freq_tbl_nom),      0 },
-[0][PVS_FAST]    = { acpu_freq_tbl_fast, sizeof(acpu_freq_tbl_fast),     0 },
+[0][PVS_SLOW]    = { acpu_freq_tbl_slow, sizeof(acpu_freq_tbl_slow), 0 },
+[0][PVS_NOMINAL] = { acpu_freq_tbl_nom,  sizeof(acpu_freq_tbl_nom),  25000 },
+[0][PVS_FAST]    = { acpu_freq_tbl_fast, sizeof(acpu_freq_tbl_fast), 25000 },
 };
 
 static struct acpuclk_krait_params acpuclk_8930ab_params __initdata = {
diff --git a/arch/arm/mach-msm/acpuclock-8960ab.c b/arch/arm/mach-msm/acpuclock-8960ab.c
index 03a2004..d2e88fb 100644
--- a/arch/arm/mach-msm/acpuclock-8960ab.c
+++ b/arch/arm/mach-msm/acpuclock-8960ab.c
@@ -105,140 +105,142 @@
 	{ }
 };
 
+#define AVS(x) .avsdscr_setting = (x)
+
 static struct acpu_level freq_tbl_PVS0[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   950000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   950000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   975000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),  1000000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),  1025000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1050000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1075000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1100000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1125000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1150000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1175000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1200000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1225000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1250000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   950000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   950000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   975000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),  1000000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),  1025000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1050000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1075000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1100000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1125000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1150000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1175000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1200000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1225000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1250000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS1[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   925000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   925000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   950000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   975000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),  1000000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1025000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1050000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1075000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1100000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1125000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1150000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1175000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1200000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1225000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   925000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   925000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   950000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   975000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),  1000000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1025000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1050000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1075000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1100000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1125000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1150000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1175000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1200000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1225000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS2[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   900000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   900000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   925000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   950000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   975000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1000000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1025000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1050000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1075000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1100000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1125000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1150000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1175000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1200000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   900000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   900000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   925000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   950000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   975000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),  1000000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1025000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1050000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1075000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1100000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1125000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1150000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1175000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1200000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS3[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   900000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   900000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   900000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   925000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   950000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   975000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1000000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1025000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1050000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1075000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1100000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1125000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1150000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1175000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   900000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   900000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   900000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   925000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   950000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   975000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),  1000000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1025000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1050000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1075000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1100000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1125000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1150000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1175000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS4[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   875000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   875000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   875000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   900000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   925000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   950000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   975000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1000000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1025000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1050000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1075000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1100000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1125000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1150000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   875000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   900000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   925000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   950000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   975000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),  1000000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1025000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1050000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1075000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1100000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1125000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1150000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS5[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   875000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   875000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   875000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   875000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   900000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   925000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   950000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),   975000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1000000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1025000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1050000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1075000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1100000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1125000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   875000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   900000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   925000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   950000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),   975000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),  1000000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1025000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1050000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1075000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1100000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1125000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct acpu_level freq_tbl_PVS6[] __initdata = {
-	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   850000 },
-	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   850000 },
-	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   850000 },
-	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   850000 },
-	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   875000 },
-	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   900000 },
-	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   925000 },
-	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),   950000 },
-	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),   975000 },
-	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1000000 },
-	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1025000 },
-	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1050000 },
-	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1075000 },
-	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1100000 },
+	{ 1, {   384000, PLL_8, 0, 0x00 }, L2(0),   850000, AVS(0x70001F) },
+	{ 1, {   486000, HFPLL, 2, 0x24 }, L2(3),   850000, AVS(0x0) },
+	{ 1, {   594000, HFPLL, 1, 0x16 }, L2(3),   850000, AVS(0x0) },
+	{ 1, {   702000, HFPLL, 1, 0x1A }, L2(3),   850000, AVS(0x0) },
+	{ 1, {   810000, HFPLL, 1, 0x1E }, L2(3),   875000, AVS(0x0) },
+	{ 1, {   918000, HFPLL, 1, 0x22 }, L2(3),   900000, AVS(0x0) },
+	{ 1, {  1026000, HFPLL, 1, 0x26 }, L2(3),   925000, AVS(0x0) },
+	{ 1, {  1134000, HFPLL, 1, 0x2A }, L2(9),   950000, AVS(0x70000D) },
+	{ 1, {  1242000, HFPLL, 1, 0x2E }, L2(9),   975000, AVS(0x0) },
+	{ 1, {  1350000, HFPLL, 1, 0x32 }, L2(9),  1000000, AVS(0x0) },
+	{ 1, {  1458000, HFPLL, 1, 0x36 }, L2(9),  1025000, AVS(0x0) },
+	{ 1, {  1566000, HFPLL, 1, 0x3A }, L2(9),  1050000, AVS(0x0) },
+	{ 1, {  1674000, HFPLL, 1, 0x3E }, L2(9),  1075000, AVS(0x0) },
+	{ 1, {  1728000, HFPLL, 1, 0x40 }, L2(9),  1100000, AVS(0x70000B) },
 	{ 0, { 0 } }
 };
 
 static struct pvs_table pvs_tables[NUM_SPEED_BINS][NUM_PVS] __initdata = {
 [0][0] = { freq_tbl_PVS0, sizeof(freq_tbl_PVS0),  0 },
-[0][1] = { freq_tbl_PVS1, sizeof(freq_tbl_PVS1),  0 },
-[0][2] = { freq_tbl_PVS2, sizeof(freq_tbl_PVS2),  0 },
-[0][3] = { freq_tbl_PVS3, sizeof(freq_tbl_PVS3),  0 },
-[0][4] = { freq_tbl_PVS4, sizeof(freq_tbl_PVS4),  0 },
-[0][5] = { freq_tbl_PVS5, sizeof(freq_tbl_PVS5),  0 },
-[0][6] = { freq_tbl_PVS6, sizeof(freq_tbl_PVS6),  0 },
+[0][1] = { freq_tbl_PVS1, sizeof(freq_tbl_PVS1),  25000 },
+[0][2] = { freq_tbl_PVS2, sizeof(freq_tbl_PVS2),  25000 },
+[0][3] = { freq_tbl_PVS3, sizeof(freq_tbl_PVS3),  25000 },
+[0][4] = { freq_tbl_PVS4, sizeof(freq_tbl_PVS4),  25000 },
+[0][5] = { freq_tbl_PVS5, sizeof(freq_tbl_PVS5),  25000 },
+[0][6] = { freq_tbl_PVS6, sizeof(freq_tbl_PVS6),  25000 },
 };
 
 static struct acpuclk_krait_params acpuclk_8960ab_params __initdata = {
diff --git a/arch/arm/mach-msm/acpuclock-8974.c b/arch/arm/mach-msm/acpuclock-8974.c
index b98fcdd..f929943 100644
--- a/arch/arm/mach-msm/acpuclock-8974.c
+++ b/arch/arm/mach-msm/acpuclock-8974.c
@@ -94,10 +94,14 @@
 };
 
 static struct msm_bus_paths bw_level_tbl[] __initdata = {
-	[0] =  BW_MBPS(552), /* At least  69 MHz on bus. */
-	[1] = BW_MBPS(1112), /* At least 139 MHz on bus. */
-	[2] = BW_MBPS(2224), /* At least 278 MHz on bus. */
-	[3] = BW_MBPS(4448), /* At least 556 MHz on bus. */
+	[0] =  BW_MBPS(600), /* At least  75 MHz on bus. */
+	[1] =  BW_MBPS(800), /* At least 100 MHz on bus. */
+	[2] = BW_MBPS(1200), /* At least 150 MHz on bus. */
+	[3] = BW_MBPS(1600), /* At least 200 MHz on bus. */
+	[4] = BW_MBPS(2224), /* At least 278 MHz on bus. */
+	[5] = BW_MBPS(3200), /* At least 400 MHz on bus. */
+	[6] = BW_MBPS(4448), /* At least 556 MHz on bus. */
+	[7] = BW_MBPS(6400), /* At least 800 MHz on bus. */
 };
 
 static struct msm_bus_scale_pdata bus_scale_data __initdata = {
@@ -113,27 +117,27 @@
 	[2]  = { {  422400, HFPLL, 2,  44 }, LVL_NOM,  1050000, 1 },
 	[3]  = { {  499200, HFPLL, 2,  52 }, LVL_NOM,  1050000, 2 },
 	[4]  = { {  576000, HFPLL, 1,  30 }, LVL_NOM,  1050000, 2 },
-	[5]  = { {  652800, HFPLL, 1,  34 }, LVL_NOM,  1050000, 2 },
-	[6]  = { {  729600, HFPLL, 1,  38 }, LVL_NOM,  1050000, 2 },
-	[7]  = { {  806400, HFPLL, 1,  42 }, LVL_NOM,  1050000, 2 },
-	[8]  = { {  883200, HFPLL, 1,  46 }, LVL_HIGH, 1050000, 2 },
-	[9]  = { {  960000, HFPLL, 1,  50 }, LVL_HIGH, 1050000, 2 },
-	[10] = { { 1036800, HFPLL, 1,  54 }, LVL_HIGH, 1050000, 3 },
-	[11] = { { 1113600, HFPLL, 1,  58 }, LVL_HIGH, 1050000, 3 },
-	[12] = { { 1190400, HFPLL, 1,  62 }, LVL_HIGH, 1050000, 3 },
-	[13] = { { 1267200, HFPLL, 1,  66 }, LVL_HIGH, 1050000, 3 },
-	[14] = { { 1344000, HFPLL, 1,  70 }, LVL_HIGH, 1050000, 3 },
-	[15] = { { 1420800, HFPLL, 1,  74 }, LVL_HIGH, 1050000, 3 },
-	[16] = { { 1497600, HFPLL, 1,  78 }, LVL_HIGH, 1050000, 3 },
-	[17] = { { 1574400, HFPLL, 1,  82 }, LVL_HIGH, 1050000, 3 },
-	[18] = { { 1651200, HFPLL, 1,  86 }, LVL_HIGH, 1050000, 3 },
-	[19] = { { 1728000, HFPLL, 1,  90 }, LVL_HIGH, 1050000, 3 },
-	[20] = { { 1804800, HFPLL, 1,  94 }, LVL_HIGH, 1050000, 3 },
-	[21] = { { 1881600, HFPLL, 1,  98 }, LVL_HIGH, 1050000, 3 },
-	[22] = { { 1958400, HFPLL, 1, 102 }, LVL_HIGH, 1050000, 3 },
-	[23] = { { 2035200, HFPLL, 1, 106 }, LVL_HIGH, 1050000, 3 },
-	[24] = { { 2112000, HFPLL, 1, 110 }, LVL_HIGH, 1050000, 3 },
-	[25] = { { 2188800, HFPLL, 1, 114 }, LVL_HIGH, 1050000, 3 },
+	[5]  = { {  652800, HFPLL, 1,  34 }, LVL_NOM,  1050000, 3 },
+	[6]  = { {  729600, HFPLL, 1,  38 }, LVL_NOM,  1050000, 3 },
+	[7]  = { {  806400, HFPLL, 1,  42 }, LVL_NOM,  1050000, 3 },
+	[8]  = { {  883200, HFPLL, 1,  46 }, LVL_HIGH, 1050000, 4 },
+	[9]  = { {  960000, HFPLL, 1,  50 }, LVL_HIGH, 1050000, 4 },
+	[10] = { { 1036800, HFPLL, 1,  54 }, LVL_HIGH, 1050000, 4 },
+	[11] = { { 1113600, HFPLL, 1,  58 }, LVL_HIGH, 1050000, 5 },
+	[12] = { { 1190400, HFPLL, 1,  62 }, LVL_HIGH, 1050000, 5 },
+	[13] = { { 1267200, HFPLL, 1,  66 }, LVL_HIGH, 1050000, 6 },
+	[14] = { { 1344000, HFPLL, 1,  70 }, LVL_HIGH, 1050000, 6 },
+	[15] = { { 1420800, HFPLL, 1,  74 }, LVL_HIGH, 1050000, 7 },
+	[16] = { { 1497600, HFPLL, 1,  78 }, LVL_HIGH, 1050000, 7 },
+	[17] = { { 1574400, HFPLL, 1,  82 }, LVL_HIGH, 1050000, 7 },
+	[18] = { { 1651200, HFPLL, 1,  86 }, LVL_HIGH, 1050000, 7 },
+	[19] = { { 1728000, HFPLL, 1,  90 }, LVL_HIGH, 1050000, 7 },
+	[20] = { { 1804800, HFPLL, 1,  94 }, LVL_HIGH, 1050000, 7 },
+	[21] = { { 1881600, HFPLL, 1,  98 }, LVL_HIGH, 1050000, 7 },
+	[22] = { { 1958400, HFPLL, 1, 102 }, LVL_HIGH, 1050000, 7 },
+	[23] = { { 2035200, HFPLL, 1, 106 }, LVL_HIGH, 1050000, 7 },
+	[24] = { { 2112000, HFPLL, 1, 110 }, LVL_HIGH, 1050000, 7 },
+	[25] = { { 2188800, HFPLL, 1, 114 }, LVL_HIGH, 1050000, 7 },
 	{ }
 };
 
diff --git a/arch/arm/mach-msm/acpuclock-8x50.c b/arch/arm/mach-msm/acpuclock-8x50.c
index 996f883..eed8000 100644
--- a/arch/arm/mach-msm/acpuclock-8x50.c
+++ b/arch/arm/mach-msm/acpuclock-8x50.c
@@ -423,14 +423,6 @@
 	}
 
 	if (reason == SETRATE_CPUFREQ) {
-#ifdef CONFIG_MSM_CPU_AVS
-		/* Notify avs before changing frequency */
-		rc = avs_adjust_freq(freq_index, 1);
-		if (rc) {
-			pr_err("Unable to increase ACPU vdd (%d)\n", rc);
-			goto out;
-		}
-#endif
 		/* Increase VDD if needed. */
 		if (tgt_s->vdd > strt_s->vdd) {
 			rc = acpuclk_set_vdd_level(tgt_s->vdd);
@@ -485,13 +477,6 @@
 	if (reason == SETRATE_PC)
 		goto out;
 
-#ifdef CONFIG_MSM_CPU_AVS
-	/* notify avs after changing frequency */
-	res = avs_adjust_freq(freq_index, 0);
-	if (res)
-		pr_warning("Unable to drop ACPU vdd (%d)\n", res);
-#endif
-
 	/* Drop VDD level if we can. */
 	if (tgt_s->vdd < strt_s->vdd) {
 		res = acpuclk_set_vdd_level(tgt_s->vdd);
@@ -658,22 +643,6 @@
 	}
 }
 
-#ifdef CONFIG_MSM_CPU_AVS
-static int __devinit acpu_avs_init(int (*set_vdd) (int), int khz)
-{
-	int i;
-	int freq_count = 0;
-	int freq_index = -1;
-
-	for (i = 0; acpu_freq_tbl[i].acpuclk_khz; i++) {
-		freq_count++;
-		if (acpu_freq_tbl[i].acpuclk_khz == khz)
-			freq_index = i;
-	}
-
-	return avs_init(set_vdd, freq_count, freq_index);
-}
-#endif
 
 static int qsd8x50_tps65023_set_dcdc1(int mVolts)
 {
@@ -728,13 +697,6 @@
 	cpufreq_table_init();
 	cpufreq_frequency_table_get_attr(freq_table, smp_processor_id());
 #endif
-#ifdef CONFIG_MSM_CPU_AVS
-	if (!acpu_avs_init(drv_state.acpu_set_vdd,
-		drv_state.current_speed->acpuclk_khz)) {
-		/* avs init successful. avs will handle voltage changes */
-		drv_state.acpu_set_vdd = NULL;
-	}
-#endif
 	return 0;
 }
 
diff --git a/arch/arm/mach-msm/avs.c b/arch/arm/mach-msm/avs.c
index 827adab..aa257ef 100644
--- a/arch/arm/mach-msm/avs.c
+++ b/arch/arm/mach-msm/avs.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -11,270 +11,86 @@
  * GNU General Public License for more details.
  */
 
-#include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/init.h>
-#include <linux/io.h>
-#include <linux/delay.h>
-#include <linux/kernel_stat.h>
-#include <linux/workqueue.h>
-#include <linux/slab.h>
-
+#include <linux/kernel.h>
+#include <asm/mach-types.h>
+#include <asm/cputype.h>
 #include "avs.h"
 
-#define AVSDSCR_INPUT 0x01004860 /* magic # from circuit designer */
-#define TSCSR_INPUT   0x00000001 /* enable temperature sense */
-
-#define TEMPRS 16                /* total number of temperature regions */
-#define GET_TEMPR() (avs_get_tscsr() >> 28) /* scale TSCSR[CTEMP] to regions */
-
-struct mutex avs_lock;
-
-static struct avs_state_s
+u32 avs_get_avscsr(void)
 {
-	u32 freq_cnt;		/* Frequencies supported list */
-	short *avs_v;		/* Dyanmically allocated storage for
-				 * 2D table of voltages over temp &
-				 * freq.  Used as a set of 1D tables.
-				 * Each table is for a single temp.
-				 * For usage see avs_get_voltage
-				 */
-	int (*set_vdd) (int);	/* Function Ptr for setting voltage */
-	int changing;		/* Clock frequency is changing */
-	u32 freq_idx;		/* Current frequency index */
-	int vdd;		/* Current ACPU voltage */
-} avs_state;
+	u32 val = 0;
 
-/*
- *  Update the AVS voltage vs frequency table, for current temperature
- *  Adjust based on the AVS delay circuit hardware status
- */
-static void avs_update_voltage_table(short *vdd_table)
+	asm volatile ("mrc p15, 7, %[avscsr], c15, c1, 7\n\t"
+			: [avscsr]"=r" (val)
+	);
+
+	return val;
+}
+EXPORT_SYMBOL(avs_get_avscsr);
+
+void avs_set_avscsr(u32 avscsr)
 {
-	u32 avscsr;
-	int cpu;
-	int vu;
-	int l2;
-	int i;
-	u32 cur_freq_idx;
-	short cur_voltage;
+	asm volatile ("mcr p15, 7, %[avscsr], c15, c1, 7\n\t"
+		      "isb\n\t"
+			:
+			: [avscsr]"r" (avscsr)
+	);
+}
+EXPORT_SYMBOL(avs_set_avscsr);
 
-	cur_freq_idx = avs_state.freq_idx;
-	cur_voltage = avs_state.vdd;
+u32 avs_get_avsdscr(void)
+{
+	u32 val = 0;
 
-	avscsr = avs_test_delays();
-	AVSDEBUG("avscsr=%x, avsdscr=%x\n", avscsr, avs_get_avsdscr());
+	asm volatile ("mrc p15, 7, %[avsdscr], c15, c0, 6\n\t"
+			: [avsdscr]"=r" (val)
+	);
 
-	/*
-	 * Read the results for the various unit's AVS delay circuits
-	 * 2=> up, 1=>down, 0=>no-change
-	 */
-	cpu = ((avscsr >> 23) & 2) + ((avscsr >> 16) & 1);
-	vu  = ((avscsr >> 28) & 2) + ((avscsr >> 21) & 1);
-	l2  = ((avscsr >> 29) & 2) + ((avscsr >> 22) & 1);
+	return val;
+}
+EXPORT_SYMBOL(avs_get_avsdscr);
 
-	if ((cpu == 3) || (vu == 3) || (l2 == 3)) {
-		printk(KERN_ERR "AVS: Dly Synth O/P error\n");
-	} else if ((cpu == 2) || (l2 == 2) || (vu == 2)) {
-		/*
-		 * even if one oscillator asks for up, increase the voltage,
-		 * as its an indication we are running outside the
-		 * critical acceptable range of v-f combination.
-		 */
-		AVSDEBUG("cpu=%d l2=%d vu=%d\n", cpu, l2, vu);
-		AVSDEBUG("Voltage up at %d\n", cur_freq_idx);
+void avs_set_avsdscr(u32 avsdscr)
+{
+	asm volatile("mcr p15, 7, %[avsdscr], c15, c0, 6\n\t"
+		     "isb\n\t"
+			:
+			: [avsdscr]"r" (avsdscr)
+	);
+}
+EXPORT_SYMBOL(avs_set_avsdscr);
 
-		if (cur_voltage >= VOLTAGE_MAX)
-			printk(KERN_ERR
-				"AVS: Voltage can not get high enough!\n");
+static void avs_enable_local(void *data)
+{
+	u32 avsdscr = (u32) data;
+	u32 avscsr_enable = 0x61;
 
-		/* Raise the voltage for all frequencies */
-		for (i = 0; i < avs_state.freq_cnt; i++) {
-			vdd_table[i] = cur_voltage + VOLTAGE_STEP;
-			if (vdd_table[i] > VOLTAGE_MAX)
-				vdd_table[i] = VOLTAGE_MAX;
-		}
-	} else if ((cpu == 1) && (l2 == 1) && (vu == 1)) {
-		if ((cur_voltage - VOLTAGE_STEP >= VOLTAGE_MIN) &&
-		    (cur_voltage <= vdd_table[cur_freq_idx])) {
-			vdd_table[cur_freq_idx] = cur_voltage - VOLTAGE_STEP;
-			AVSDEBUG("Voltage down for %d and lower levels\n",
-				cur_freq_idx);
-
-			/* clamp to this voltage for all lower levels */
-			for (i = 0; i < cur_freq_idx; i++) {
-				if (vdd_table[i] > vdd_table[cur_freq_idx])
-					vdd_table[i] = vdd_table[cur_freq_idx];
-			}
-		}
-	}
+	avs_set_avsdscr(avsdscr);
+	avs_set_avscsr(avscsr_enable);
 }
 
-/*
- * Return the voltage for the target performance freq_idx and optionally
- * use AVS hardware to check the present voltage freq_idx
- */
-static short avs_get_target_voltage(int freq_idx, bool update_table)
+static void avs_disable_local(void *data)
 {
-	unsigned cur_tempr = GET_TEMPR();
-	unsigned temp_index = cur_tempr*avs_state.freq_cnt;
-
-	/* Table of voltages vs frequencies for this temp */
-	short *vdd_table = avs_state.avs_v + temp_index;
-
-	if (update_table)
-		avs_update_voltage_table(vdd_table);
-
-	return vdd_table[freq_idx];
+	avs_set_avscsr(0);
 }
 
-
-/*
- * Set the voltage for the freq_idx and optionally
- * use AVS hardware to update the voltage
- */
-static int avs_set_target_voltage(int freq_idx, bool update_table)
+void avs_enable(int cpu, u32 avsdscr)
 {
-	int rc = 0;
-	int new_voltage = avs_get_target_voltage(freq_idx, update_table);
-	if (avs_state.vdd != new_voltage) {
-		AVSDEBUG("AVS setting V to %d mV @%d\n",
-			new_voltage, freq_idx);
-		rc = avs_state.set_vdd(new_voltage);
-		if (rc)
-			return rc;
-		avs_state.vdd = new_voltage;
-	}
-	return rc;
-}
+	int ret;
 
-/*
- * Notify avs of clk frquency transition begin & end
- */
-int avs_adjust_freq(u32 freq_idx, int begin)
+	ret = smp_call_function_single(cpu, avs_enable_local,
+			(void *)avsdscr, true);
+	WARN_ON(ret);
+}
+EXPORT_SYMBOL(avs_enable);
+
+void avs_disable(int cpu)
 {
-	int rc = 0;
+	int ret;
 
-	if (!avs_state.set_vdd) {
-		/* AVS not initialized */
-		return 0;
-	}
-
-	if (freq_idx >= avs_state.freq_cnt) {
-		AVSDEBUG("Out of range :%d\n", freq_idx);
-		return -EINVAL;
-	}
-
-	mutex_lock(&avs_lock);
-	if ((begin && (freq_idx > avs_state.freq_idx)) ||
-	    (!begin && (freq_idx < avs_state.freq_idx))) {
-		/* Update voltage before increasing frequency &
-		 * after decreasing frequency
-		 */
-		rc = avs_set_target_voltage(freq_idx, 0);
-		if (rc)
-			goto aaf_out;
-
-		avs_state.freq_idx = freq_idx;
-	}
-	avs_state.changing = begin;
-aaf_out:
-	mutex_unlock(&avs_lock);
-
-	return rc;
+	ret = smp_call_function_single(cpu, avs_disable_local,
+			(void *) 0, true);
+	WARN_ON(ret);
 }
-
-
-static struct delayed_work avs_work;
-static struct workqueue_struct  *kavs_wq;
-#define AVS_DELAY ((CONFIG_HZ * 50 + 999) / 1000)
-
-static void do_avs_timer(struct work_struct *work)
-{
-	int cur_freq_idx;
-
-	mutex_lock(&avs_lock);
-	if (!avs_state.changing) {
-		/* Only adjust the voltage if clk is stable */
-		cur_freq_idx = avs_state.freq_idx;
-		avs_set_target_voltage(cur_freq_idx, 1);
-	}
-	mutex_unlock(&avs_lock);
-	queue_delayed_work_on(0, kavs_wq, &avs_work, AVS_DELAY);
-}
-
-
-static void __init avs_timer_init(void)
-{
-	INIT_DELAYED_WORK_DEFERRABLE(&avs_work, do_avs_timer);
-	queue_delayed_work_on(0, kavs_wq, &avs_work, AVS_DELAY);
-}
-
-static void __exit avs_timer_exit(void)
-{
-	cancel_delayed_work(&avs_work);
-}
-
-static int __init avs_work_init(void)
-{
-	kavs_wq = create_workqueue("avs");
-	if (!kavs_wq) {
-		printk(KERN_ERR "AVS initialization failed\n");
-		return -EFAULT;
-	}
-	avs_timer_init();
-
-	return 1;
-}
-
-static void __exit avs_work_exit(void)
-{
-	avs_timer_exit();
-	destroy_workqueue(kavs_wq);
-}
-
-int __init avs_init(int (*set_vdd)(int), u32 freq_cnt, u32 freq_idx)
-{
-	int i;
-
-	mutex_init(&avs_lock);
-
-	if (freq_cnt == 0)
-		return -EINVAL;
-
-	avs_state.freq_cnt = freq_cnt;
-
-	if (freq_idx >= avs_state.freq_cnt)
-		return -EINVAL;
-
-	avs_state.avs_v = kmalloc(TEMPRS * avs_state.freq_cnt *
-		sizeof(avs_state.avs_v[0]), GFP_KERNEL);
-
-	if (avs_state.avs_v == 0)
-		return -ENOMEM;
-
-	for (i = 0; i < TEMPRS*avs_state.freq_cnt; i++)
-		avs_state.avs_v[i] = VOLTAGE_MAX;
-
-	avs_reset_delays(AVSDSCR_INPUT);
-	avs_set_tscsr(TSCSR_INPUT);
-
-	avs_state.set_vdd = set_vdd;
-	avs_state.changing = 0;
-	avs_state.freq_idx = -1;
-	avs_state.vdd = -1;
-	avs_adjust_freq(freq_idx, 0);
-
-	avs_work_init();
-
-	return 0;
-}
-
-void __exit avs_exit()
-{
-	avs_work_exit();
-
-	kfree(avs_state.avs_v);
-}
-
-
+EXPORT_SYMBOL(avs_disable);
diff --git a/arch/arm/mach-msm/avs.h b/arch/arm/mach-msm/avs.h
index e87bded..f8b311c 100644
--- a/arch/arm/mach-msm/avs.h
+++ b/arch/arm/mach-msm/avs.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2009, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2009,2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -14,63 +14,25 @@
 #ifndef AVS_H
 #define AVS_H
 
-#define VOLTAGE_MIN  1000 /* mV */
-#define VOLTAGE_MAX  1250
-#define VOLTAGE_STEP 25
-
-int __init avs_init(int (*set_vdd)(int), u32 freq_cnt, u32 freq_idx);
-void __exit avs_exit(void);
-
-int avs_adjust_freq(u32 freq_index, int begin);
-
-/* Routines exported from avs_hw.S */
-#ifdef CONFIG_MSM_CPU_AVS
-u32 avs_test_delays(void);
-#else
-static inline u32 avs_test_delays(void)
-{ return 0; }
-#endif
-
 #ifdef CONFIG_MSM_AVS_HW
-u32 avs_reset_delays(u32 avsdscr);
 u32 avs_get_avscsr(void);
+void avs_set_avscsr(u32 avscsr);
 u32 avs_get_avsdscr(void);
-u32 avs_get_tscsr(void);
-void avs_set_tscsr(u32 to_tscsr);
-u32 avs_disable(void);
-void avs_enable(u32 avscsr);
+void avs_set_avsdscr(u32 avsdscr);
+void avs_disable(int cpu);
+void avs_enable(int cpu, u32 avsdscr);
 #else
-static inline u32 avs_reset_delays(u32 avsdscr)
-{ return 0; }
 static inline u32 avs_get_avscsr(void)
 { return 0; }
+static inline void avs_set_avscsr(u32 avscsr) {}
 static inline u32 avs_get_avsdscr(void)
 { return 0; }
-static inline u32 avs_get_tscsr(void)
-{ return 0; }
-static inline void avs_set_tscsr(u32 to_tscsr) {}
-static inline u32 avs_disable(void)
-{return 0; }
-static inline void avs_enable(u32 avscsr) {}
+static inline void avs_set_avsdscr(u32 avsdscr) {}
+static inline void avs_disable(int cpu) {}
+static inline void avs_enable(int cpu, u32 avsdscr) {}
 #endif
 
-/*#define AVSDEBUG(x...) pr_info("AVS: " x);*/
-#define AVSDEBUG(...)
+#define AVS_DISABLE(cpu) avs_disable(cpu)
+#define AVS_ENABLE(cpu, x) avs_enable(cpu, x)
 
-#define AVS_DISABLE(cpu) do {			\
-		if (get_cpu() == (cpu))		\
-			avs_disable();		\
-		put_cpu();			\
-	} while (0);
-
-/* AVSCSR(0x61) to enable CPU, V and L2 AVS module */
-
-#define AVS_ENABLE(cpu, x) do {			\
-		if (get_cpu() == (cpu)) {       \
-			avs_reset_delays((x));	\
-			avs_enable(0x61);	\
-		}				\
-		put_cpu();			\
-	} while (0);
-
-#endif /* AVS_H */
+#endif
diff --git a/arch/arm/mach-msm/avs_hw.S b/arch/arm/mach-msm/avs_hw.S
deleted file mode 100644
index efb9c47..0000000
--- a/arch/arm/mach-msm/avs_hw.S
+++ /dev/null
@@ -1,127 +0,0 @@
-/*
- * Copyright (c) 2009, Code Aurora Forum. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-	.text
-
-#ifdef CONFIG_MSM_CPU_AVS
-	.global avs_test_delays
-avs_test_delays:
-
-/*      Read r1=CPMR and enable Never Sleep for VSLPDLY */
-		mrc  p15, 7, r1, c15, c0, 5
-		orr  r12, r1, #3, 24
-		mcr  p15, 7, r12, c15, c0, 5
-
-/*      Read r2=CPACR and enable full access to CP10 and CP11 space */
-		mrc p15, 0, r2, c1, c0, 2
-		orr r12, r2, #(0xf << 20)
-		mcr p15, 0, r12, c1, c0, 2
-		isb
-
-/*      Read r3=FPEXC and or in FP enable, VFP/ASE enable = FPEXC[30]; */
-		fmrx r3, fpexc
-		orr  r12, r3, #1, 2
-		fmxr fpexc, r12
-
-/*
- *      Do floating-point operations to prime the VFP pipeline.   Use
- *      fcpyd d0, d0 as a floating point nop.  This avoids changing VFP
- *      state.
- */
-		fcpyd d0, d0
-		fcpyd d0, d0
-		fcpyd d0, d0
-
-/*      Read r0=AVSCSR to get status from CPU, VFP, and L2 ring oscillators */
-		mrc p15, 7, r0, c15, c1, 7
-
-/*      Restore FPEXC */
-		fmxr fpexc, r3
-
-/*      Restore CPACR */
-                MCR p15, 0, r2, c1, c0, 2
-
-/*      Restore CPMR */
-		mcr p15, 7, r1, c15, c0, 5
-                isb
-
-		bx lr
-#endif
-
-
-	.global avs_get_avscsr
-/*      Read r0=AVSCSR to get status from CPU, VFP, and L2 ring oscillators */
-
-avs_get_avscsr:
-		mrc p15, 7, r0, c15, c1, 7
-                bx lr
-
-        .global avs_get_avsdscr
-/*      Read r0=AVSDSCR to get the AVS Delay Synthesizer control settings */
-
-avs_get_avsdscr:
-		mrc p15, 7, r0, c15, c0, 6
-                bx lr
-
-
-
-
-	.global avs_get_tscsr
-/*      Read r0=TSCSR to get temperature sensor control and status */
-
-avs_get_tscsr:
-		mrc p15, 7, r0, c15, c1, 0
-                bx lr
-
-        .global avs_set_tscsr
-/*      Write TSCSR=r0 to set temperature sensor control and status  */
-
-avs_set_tscsr:
-		mcr p15, 7, r0, c15, c1, 0
-                bx lr
-
-
-
-
-
-	.global avs_reset_delays
-avs_reset_delays:
-
-/*      AVSDSCR(dly) to program delay */
-		mcr p15, 7, r0, c15, c0, 6
-
-/*      Read r0=AVSDSCR */
-		mrc p15, 7, r0, c15, c0, 6
-		bx lr
-
-	.global avs_enable
-avs_enable:
-/*	Restore the avs_scr register */
-		mcr p15, 7, r0, c15, c1, 7
-		bx lr
-
-        .global avs_disable
-avs_disable:
-
-/*	Get the AVSCSR value */
-		mrc p15, 7, r0, c15, c1, 7
-/*      Clear AVSCSR */
-		mov r1, #0
-/*      Write AVSCSR */
-		mcr p15, 7, r1, c15, c1, 7
-
-		bx lr
-
-	.end
-
-
diff --git a/arch/arm/mach-msm/board-8064-gpiomux.c b/arch/arm/mach-msm/board-8064-gpiomux.c
index cd3e636..9e34f47 100644
--- a/arch/arm/mach-msm/board-8064-gpiomux.c
+++ b/arch/arm/mach-msm/board-8064-gpiomux.c
@@ -1647,7 +1647,8 @@
 		msm_gpiomux_install(mpq8064_mi2s_configs,
 			ARRAY_SIZE(mpq8064_mi2s_configs));
 
-	msm_gpiomux_install(apq8064_ext_regulator_configs,
+	if (!machine_is_mpq8064_hrd())
+		msm_gpiomux_install(apq8064_ext_regulator_configs,
 			ARRAY_SIZE(apq8064_ext_regulator_configs));
 
 	if (machine_is_apq8064_mtp()) {
diff --git a/arch/arm/mach-msm/board-8064-gpu.c b/arch/arm/mach-msm/board-8064-gpu.c
index 38ac83e..6b15883 100644
--- a/arch/arm/mach-msm/board-8064-gpu.c
+++ b/arch/arm/mach-msm/board-8064-gpu.c
@@ -51,7 +51,7 @@
 		.ss_win_size_min_us	= 1000000,
 		.ss_win_size_max_us	= 1000000,
 		.ss_util_pct		= 95,
-		.ss_iobusy_conv		= 100,
+		.ss_no_corr_below_freq	= 0,
 	},
 
 	.energy_coeffs		= {
diff --git a/arch/arm/mach-msm/board-8064-regulator.c b/arch/arm/mach-msm/board-8064-regulator.c
index a66495d..1a8e7e4 100644
--- a/arch/arm/mach-msm/board-8064-regulator.c
+++ b/arch/arm/mach-msm/board-8064-regulator.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2011-2012, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -97,6 +97,7 @@
 };
 VREG_CONSUMERS(L14) = {
 	REGULATOR_SUPPLY("8921_l14",		NULL),
+	REGULATOR_SUPPLY("vreg_xoadc",		"pm8921-charger"),
 };
 VREG_CONSUMERS(L15) = {
 	REGULATOR_SUPPLY("8921_l15",		NULL),
@@ -259,6 +260,7 @@
 	REGULATOR_SUPPLY("dsi1_vccs_3p3v",      "mipi_dsi.1"),
 	REGULATOR_SUPPLY("hdmi_mux_vdd",        "hdmi_msm.0"),
 	REGULATOR_SUPPLY("pcie_ext_3p3v",       "msm_pcie"),
+	REGULATOR_SUPPLY("sata_ext_3p3v",       "ahci.0"),
 };
 VREG_CONSUMERS(EXT_TS_SW) = {
 	REGULATOR_SUPPLY("ext_ts_sw",		NULL),
diff --git a/arch/arm/mach-msm/board-8064.c b/arch/arm/mach-msm/board-8064.c
index a8ad0d6..549a029 100644
--- a/arch/arm/mach-msm/board-8064.c
+++ b/arch/arm/mach-msm/board-8064.c
@@ -88,6 +88,7 @@
 #include "pm-boot.h"
 #include "devices-msm8x60.h"
 #include "smd_private.h"
+#include "platsmp.h"
 
 #define MHL_GPIO_INT           30
 #define MHL_GPIO_RESET         35
@@ -297,6 +298,8 @@
 	.reusable = FMEM_ENABLED,
 	.mem_is_fmem = FMEM_ENABLED,
 	.fixed_position = FIXED_MIDDLE,
+	.is_cma = 1,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_cp_heap_pdata cp_mfc_apq8064_ion_pdata = {
@@ -305,6 +308,7 @@
 	.reusable = 0,
 	.mem_is_fmem = FMEM_ENABLED,
 	.fixed_position = FIXED_HIGH,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_co_heap_pdata co_apq8064_ion_pdata = {
@@ -1634,10 +1638,10 @@
 static struct cyttsp_platform_data cyttsp_pdata = {
 	.panel_maxx = 634,
 	.panel_maxy = 1166,
-	.disp_maxx = 599,
-	.disp_maxy = 1023,
-	.disp_minx = 0,
-	.disp_miny = 0,
+	.disp_minx = 18,
+	.disp_maxx = 617,
+	.disp_miny = 18,
+	.disp_maxy = 1041,
 	.flags = 0x01,
 	.gen = CY_GEN3,
 	.use_st = CY_USE_ST,
@@ -2526,6 +2530,13 @@
 	&apq8064_device_ssbi_pmic2,
 };
 
+static struct platform_device *pm8921_mpq_hrd_common_devices[] __initdata = {
+	&apq8064_device_ext_5v_vreg,
+	&apq8064_device_ext_mpp8_vreg,
+	&apq8064_device_ssbi_pmic1,
+	&apq8064_device_ssbi_pmic2,
+};
+
 static struct platform_device *pm8917_common_devices[] __initdata = {
 	&apq8064_device_ext_mpp8_vreg,
 	&apq8064_device_ext_3p3v_vreg,
@@ -2718,6 +2729,11 @@
 	.id	= -1,
 };
 
+static struct platform_device sp_input_loopback_pdev = {
+	.name	= "sp-user-input",
+	.id	= -1,
+};
+
 static int rf4ce_gpio_init(void)
 {
 	if (!machine_is_mpq8064_cdp() &&
@@ -2799,6 +2815,7 @@
 #endif
 	&rc_input_loopback_pdev,
 	&mpq8064_device_qup_spi_gsbi6,
+	&sp_input_loopback_pdev,
 };
 
 static struct msm_spi_platform_data apq8064_qup_spi_gsbi5_pdata = {
@@ -3245,6 +3262,11 @@
 	},
 };
 
+static struct platform_device msm_dev_avtimer_device = {
+	.name = "dev_avtimer",
+	.dev = { .platform_data = &dev_avtimer_pdata },
+};
+
 /* Sensors DSPS platform data */
 #define DSPS_PIL_GENERIC_NAME		"dsps"
 static void __init apq8064_init_dsps(void)
@@ -3544,9 +3566,14 @@
 
 	platform_add_devices(early_common_devices,
 				ARRAY_SIZE(early_common_devices));
-	if (socinfo_get_pmic_model() != PMIC_MODEL_PM8917)
-		platform_add_devices(pm8921_common_devices,
-					ARRAY_SIZE(pm8921_common_devices));
+	if (socinfo_get_pmic_model() != PMIC_MODEL_PM8917) {
+		if (!machine_is_mpq8064_hrd())
+			platform_add_devices(pm8921_common_devices,
+				ARRAY_SIZE(pm8921_common_devices));
+		else
+			platform_add_devices(pm8921_mpq_hrd_common_devices,
+				ARRAY_SIZE(pm8921_mpq_hrd_common_devices));
+	}
 	else
 		platform_add_devices(pm8917_common_devices,
 					ARRAY_SIZE(pm8917_common_devices));
@@ -3684,6 +3711,9 @@
 		platform_device_register(&mpq_keypad_device);
 	} else if (machine_is_mpq8064_hrd())
 		platform_device_register(&mpq_hrd_keys_pdev);
+	if (machine_is_mpq8064_cdp() || machine_is_mpq8064_hrd() ||
+		machine_is_mpq8064_dtv())
+		platform_device_register(&msm_dev_avtimer_device);
 }
 
 MACHINE_START(APQ8064_CDP, "QCT APQ8064 CDP")
@@ -3696,6 +3726,7 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(APQ8064_MTP, "QCT APQ8064 MTP")
@@ -3708,6 +3739,7 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(APQ8064_LIQUID, "QCT APQ8064 LIQUID")
@@ -3720,6 +3752,7 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MPQ8064_CDP, "QCT MPQ8064 CDP")
@@ -3732,6 +3765,7 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MPQ8064_HRD, "QCT MPQ8064 HRD")
@@ -3744,6 +3778,7 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MPQ8064_DTV, "QCT MPQ8064 DTV")
@@ -3756,4 +3791,5 @@
 	.init_early = apq8064_allocate_memory_regions,
 	.init_very_early = apq8064_early_reserve,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8064.h b/arch/arm/mach-msm/board-8064.h
index 5d6f988..2bc0981 100644
--- a/arch/arm/mach-msm/board-8064.h
+++ b/arch/arm/mach-msm/board-8064.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2012, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -16,6 +16,7 @@
 #include <linux/regulator/msm-gpio-regulator.h>
 #include <linux/mfd/pm8xxx/pm8921.h>
 #include <linux/mfd/pm8xxx/pm8821.h>
+#include <linux/ahci_platform.h>
 #include <mach/msm_memtypes.h>
 #include <mach/irqs.h>
 #include <mach/rpm-regulator.h>
@@ -80,6 +81,7 @@
 		struct mmc_platform_data *plat);
 
 void apq8064_init_mmc(void);
+int __init apq8064_add_ahci(struct ahci_platform_data *platd);
 void apq8064_init_gpiomux(void);
 void apq8064_init_pmic(void);
 
diff --git a/arch/arm/mach-msm/board-8092.c b/arch/arm/mach-msm/board-8092.c
index 3e31f68..cbd257f 100644
--- a/arch/arm/mach-msm/board-8092.c
+++ b/arch/arm/mach-msm/board-8092.c
@@ -31,6 +31,7 @@
 
 #include "board-dt.h"
 #include "clock.h"
+#include "platsmp.h"
 
 static struct clk_lookup msm_clocks_dummy[] = {
 	CLK_DUMMY("core_clk",   BLSP1_UART_CLK, "msm_serial_hsl.0", OFF),
@@ -117,4 +118,5 @@
 	.dt_compat = mpq8092_dt_match,
 	.reserve = mpq8092_dt_reserve,
 	.init_very_early = mpq8092_early_memory,
+	.smp = &msm8974_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8226.c b/arch/arm/mach-msm/board-8226.c
index fde82f4..a5cb481 100644
--- a/arch/arm/mach-msm/board-8226.c
+++ b/arch/arm/mach-msm/board-8226.c
@@ -43,6 +43,7 @@
 #include <mach/clk-provider.h>
 #include "board-dt.h"
 #include "clock.h"
+#include "platsmp.h"
 
 static struct memtype_reserve msm8226_reserve_table[] __initdata = {
 	[MEMTYPE_SMI] = {
@@ -103,7 +104,6 @@
 	msm_reserve();
 }
 
-
 void __init msm8226_init(void)
 {
 	struct of_dev_auxdata *adata = msm8226_auxdata_lookup;
@@ -133,4 +133,5 @@
 	.reserve = msm8226_reserve,
 	.init_very_early = msm8226_early_memory,
 	.restart = msm_restart,
+	.smp = &arm_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8910.c b/arch/arm/mach-msm/board-8910.c
index b4a95ee..2ff4567 100644
--- a/arch/arm/mach-msm/board-8910.c
+++ b/arch/arm/mach-msm/board-8910.c
@@ -41,6 +41,7 @@
 #include <mach/clk-provider.h>
 #include "board-dt.h"
 #include "clock.h"
+#include "platsmp.h"
 
 static struct memtype_reserve msm8910_reserve_table[] __initdata = {
 	[MEMTYPE_SMI] = {
@@ -58,25 +59,6 @@
 	return MEMTYPE_EBI1;
 }
 
-static struct clk_lookup msm_clocks_dummy[] = {
-	CLK_DUMMY("core_clk",   BLSP1_UART_CLK, "f991f000.serial", OFF),
-	CLK_DUMMY("iface_clk",  BLSP1_UART_CLK, "f991f000.serial", OFF),
-	CLK_DUMMY("iface_clk",  HSUSB_IFACE_CLK, "f9a55000.usb", OFF),
-	CLK_DUMMY("core_clk",	HSUSB_CORE_CLK, "f9a55000.usb", OFF),
-	CLK_DUMMY("iface_clk",	NULL,		"msm_sdcc.1", OFF),
-	CLK_DUMMY("core_clk",	NULL,		"msm_sdcc.1", OFF),
-	CLK_DUMMY("bus_clk",	NULL,		"msm_sdcc.1", OFF),
-	CLK_DUMMY("iface_clk",	NULL,		"msm_sdcc.2", OFF),
-	CLK_DUMMY("core_clk",	NULL,		"msm_sdcc.2", OFF),
-	CLK_DUMMY("bus_clk",	NULL,		"msm_sdcc.2", OFF),
-	CLK_DUMMY("dfab_clk",	DFAB_CLK,	"msm_sps", OFF),
-};
-
-static struct clock_init_data msm_dummy_clock_init_data __initdata = {
-	.table = msm_clocks_dummy,
-	.size = ARRAY_SIZE(msm_clocks_dummy),
-};
-
 static struct of_dev_auxdata msm8910_auxdata_lookup[] __initdata = {
 	OF_DEV_AUXDATA("qcom,msm-sdcc", 0xF9824000, \
 			"msm_sdcc.1", NULL),
@@ -106,7 +88,11 @@
 	struct of_dev_auxdata *adata = msm8910_auxdata_lookup;
 
 	msm8910_init_gpiomux();
-	msm_clock_init(&msm_dummy_clock_init_data);
+
+	if (machine_is_msm8910_rumi())
+		msm_clock_init(&msm8910_rumi_clock_init_data);
+	else
+		msm_clock_init(&msm8910_clock_init_data);
 
 	if (socinfo_init() < 0)
 		pr_err("%s: socinfo_init() failed\n", __func__);
@@ -128,5 +114,6 @@
 	.dt_compat = msm8910_dt_match,
 	.restart = msm_restart,
 	.reserve = msm8910_reserve,
-	.init_very_early = msm8910_early_memory
+	.init_very_early = msm8910_early_memory,
+	.smp = &arm_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8930-regulator-pm8917.c b/arch/arm/mach-msm/board-8930-regulator-pm8917.c
index 9a2967a..9d0d3ee 100644
--- a/arch/arm/mach-msm/board-8930-regulator-pm8917.c
+++ b/arch/arm/mach-msm/board-8930-regulator-pm8917.c
@@ -552,8 +552,8 @@
 	/*	ID a_on pd ss min_uV   max_uV  supply sys_uA  freq  fm  ss_fm */
 	RPM_SMPS(S1, 1, 1, 0, 1300000, 1300000, NULL, 100000, 3p20, NONE, NONE),
 	RPM_SMPS(S2, 0, 1, 0, 1300000, 1300000, NULL,      0, 1p60, NONE, NONE),
-	RPM_SMPS(S3, 0, 1, 1,  500000, 1150000, NULL, 100000, 4p80, AUTO, LPM),
-	RPM_SMPS(S4, 1, 1, 0, 1800000, 1800000, NULL, 100000, 1p60, AUTO, LPM),
+	RPM_SMPS(S3, 0, 1, 1,  500000, 1150000, NULL, 100000, 4p80, NONE, NONE),
+	RPM_SMPS(S4, 1, 1, 0, 1800000, 1800000, NULL, 100000, 1p60, NONE, NONE),
 	RPM_SMPS(S7, 0, 1, 0, 1150000, 1150000, NULL, 100000, 3p20, AUTO, AUTO),
 	RPM_SMPS(S8, 1, 1, 1, 2050000, 2050000, NULL, 100000, 1p60, NONE, NONE),
 
diff --git a/arch/arm/mach-msm/board-8930.c b/arch/arm/mach-msm/board-8930.c
index f0c33eb..1a953da 100644
--- a/arch/arm/mach-msm/board-8930.c
+++ b/arch/arm/mach-msm/board-8930.c
@@ -103,6 +103,7 @@
 #include "msm_watchdog.h"
 #include "board-8930.h"
 #include "acpuclock-krait.h"
+#include "platsmp.h"
 
 static struct platform_device msm_fm_platform_init = {
 	.name = "iris_fm",
@@ -342,6 +343,8 @@
 	.reusable = FMEM_ENABLED,
 	.mem_is_fmem = FMEM_ENABLED,
 	.fixed_position = FIXED_MIDDLE,
+	.is_cma	= 1,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_cp_heap_pdata cp_mfc_msm8930_ion_pdata = {
@@ -350,6 +353,7 @@
 	.reusable = 0,
 	.mem_is_fmem = FMEM_ENABLED,
 	.fixed_position = FIXED_HIGH,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_co_heap_pdata co_msm8930_ion_pdata = {
@@ -2969,6 +2973,7 @@
 	.init_early = msm8930_allocate_memory_regions,
 	.init_very_early = msm8930_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8930_MTP, "QCT MSM8930 MTP")
@@ -2981,6 +2986,7 @@
 	.init_early = msm8930_allocate_memory_regions,
 	.init_very_early = msm8930_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8930_FLUID, "QCT MSM8930 FLUID")
@@ -2993,6 +2999,7 @@
 	.init_early = msm8930_allocate_memory_regions,
 	.init_very_early = msm8930_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8627_CDP, "QCT MSM8627 CDP")
@@ -3005,6 +3012,7 @@
 	.init_early = msm8930_allocate_memory_regions,
 	.init_very_early = msm8930_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8627_MTP, "QCT MSM8627 MTP")
@@ -3017,4 +3025,5 @@
 	.init_early = msm8930_allocate_memory_regions,
 	.init_very_early = msm8930_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8960-camera.c b/arch/arm/mach-msm/board-8960-camera.c
index 7a2e9e1..3853e4c 100644
--- a/arch/arm/mach-msm/board-8960-camera.c
+++ b/arch/arm/mach-msm/board-8960-camera.c
@@ -15,6 +15,7 @@
 #include <linux/gpio.h>
 #include <mach/camera.h>
 #include <mach/msm_bus_board.h>
+#include <mach/socinfo.h>
 #include <mach/gpiomux.h>
 #include "devices.h"
 #include "board-8960.h"
@@ -182,6 +183,23 @@
 	},
 };
 
+static struct msm_gpiomux_config msm8960_cam_2d_configs_sglte[] = {
+	{
+		.gpio = 20,
+		.settings = {
+			[GPIOMUX_ACTIVE]    = &cam_settings[3],
+			[GPIOMUX_SUSPENDED] = &cam_settings[8],
+		},
+	},
+	{
+		.gpio = 21,
+		.settings = {
+			[GPIOMUX_ACTIVE]    = &cam_settings[3],
+			[GPIOMUX_SUSPENDED] = &cam_settings[8],
+		},
+	},
+};
+
 #define VFE_CAMIF_TIMER1_GPIO 2
 #define VFE_CAMIF_TIMER2_GPIO 3
 #define VFE_CAMIF_TIMER3_GPIO_INT 4
@@ -828,6 +846,16 @@
 
 void __init msm8960_init_cam(void)
 {
+	if (socinfo_get_platform_subtype() == PLATFORM_SUBTYPE_SGLTE) {
+		msm_8960_front_cam_gpio_conf.cam_gpiomux_conf_tbl =
+			msm8960_cam_2d_configs_sglte;
+		msm_8960_front_cam_gpio_conf.cam_gpiomux_conf_tbl_size =
+			ARRAY_SIZE(msm8960_cam_2d_configs_sglte);
+		msm_8960_back_cam_gpio_conf.cam_gpiomux_conf_tbl =
+			msm8960_cam_2d_configs_sglte;
+		msm_8960_back_cam_gpio_conf.cam_gpiomux_conf_tbl_size =
+			ARRAY_SIZE(msm8960_cam_2d_configs_sglte);
+	}
 	msm_gpiomux_install(msm8960_cam_common_configs,
 			ARRAY_SIZE(msm8960_cam_common_configs));
 
diff --git a/arch/arm/mach-msm/board-8960-regulator.c b/arch/arm/mach-msm/board-8960-regulator.c
index 397411d..22c16c7 100644
--- a/arch/arm/mach-msm/board-8960-regulator.c
+++ b/arch/arm/mach-msm/board-8960-regulator.c
@@ -92,6 +92,7 @@
 VREG_CONSUMERS(L14) = {
 	REGULATOR_SUPPLY("8921_l14",		NULL),
 	REGULATOR_SUPPLY("pa_therm",		"pm8xxx-adc"),
+	REGULATOR_SUPPLY("vreg_xoadc",		"pm8921-charger"),
 };
 VREG_CONSUMERS(L15) = {
 	REGULATOR_SUPPLY("8921_l15",		NULL),
diff --git a/arch/arm/mach-msm/board-8960.c b/arch/arm/mach-msm/board-8960.c
index 64ada24..9b19810 100644
--- a/arch/arm/mach-msm/board-8960.c
+++ b/arch/arm/mach-msm/board-8960.c
@@ -103,6 +103,7 @@
 #include "smd_private.h"
 #include "pm-boot.h"
 #include "msm_watchdog.h"
+#include "platsmp.h"
 
 #if defined(CONFIG_BT) && defined(CONFIG_BT_HCIUART_ATH3K)
 #include <linux/wlan_plat.h>
@@ -365,6 +366,8 @@
 	.fixed_position = FIXED_MIDDLE,
 	.iommu_map_all = 1,
 	.iommu_2x_map_domain = VIDEO_DOMAIN,
+	.is_cma = 1,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_cp_heap_pdata cp_mfc_msm8960_ion_pdata = {
@@ -373,6 +376,7 @@
 	.reusable = 0,
 	.mem_is_fmem = FMEM_ENABLED,
 	.fixed_position = FIXED_HIGH,
+	.no_nonsecure_alloc = 1,
 };
 
 static struct ion_co_heap_pdata co_msm8960_ion_pdata = {
@@ -748,6 +752,29 @@
 #endif
 }
 
+static void ion_adjust_secure_allocation(void)
+{
+	int i;
+
+	for (i = 0; i < msm8960_ion_pdata.nr; i++) {
+		struct ion_platform_heap *heap =
+			&(msm8960_ion_pdata.heaps[i]);
+
+
+		if (heap->extra_data) {
+			switch ((int) heap->type) {
+			case ION_HEAP_TYPE_CP:
+				if (cpu_is_msm8960()) {
+					((struct ion_cp_heap_pdata *)
+					heap->extra_data)->no_nonsecure_alloc =
+						0;
+				}
+
+			}
+		}
+	}
+}
+
 static void __init reserve_mdp_memory(void)
 {
 	msm8960_mdp_writeback(msm8960_reserve_table);
@@ -3328,13 +3355,6 @@
 {
 	int i;
 
-	/* Reset the AVS registers until we have support for AVS */
-	for (i = 0; i < ARRAY_SIZE(msm_spm_data); i++) {
-		struct msm_spm_platform_data *pdata = &msm_spm_data[i];
-		pdata->reg_init_values[MSM_SPM_REG_SAW2_AVS_CTL] = 0;
-		pdata->reg_init_values[MSM_SPM_REG_SAW2_AVS_HYSTERESIS] = 0;
-	}
-
 	/* Update the SPM sequences for SPC and PC */
 	for (i = 0; i < ARRAY_SIZE(msm_spm_data); i++) {
 		int j;
@@ -3475,6 +3495,7 @@
 		platform_device_register(&mdm_sglte_device);
 	}
 	msm_pm_set_tz_retention_flag(1);
+	ion_adjust_secure_allocation();
 }
 
 MACHINE_START(MSM8960_CDP, "QCT MSM8960 CDP")
@@ -3487,6 +3508,7 @@
 	.init_early = msm8960_allocate_memory_regions,
 	.init_very_early = msm8960_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8960_MTP, "QCT MSM8960 MTP")
@@ -3499,6 +3521,7 @@
 	.init_early = msm8960_allocate_memory_regions,
 	.init_very_early = msm8960_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8960_FLUID, "QCT MSM8960 FLUID")
@@ -3511,6 +3534,7 @@
 	.init_early = msm8960_allocate_memory_regions,
 	.init_very_early = msm8960_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8960_LIQUID, "QCT MSM8960 LIQUID")
@@ -3523,4 +3547,5 @@
 	.init_early = msm8960_allocate_memory_regions,
 	.init_very_early = msm8960_early_memory,
 	.restart = msm_restart,
+	.smp = &msm8960_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-8974-gpiomux.c b/arch/arm/mach-msm/board-8974-gpiomux.c
index 0f6c000..28d50cc 100644
--- a/arch/arm/mach-msm/board-8974-gpiomux.c
+++ b/arch/arm/mach-msm/board-8974-gpiomux.c
@@ -68,6 +68,12 @@
 };
 #endif
 
+static struct gpiomux_setting gpio_epm_config = {
+	.func = GPIOMUX_FUNC_GPIO,
+	.drv  = GPIOMUX_DRV_2MA,
+	.pull = GPIOMUX_PULL_DOWN,
+};
+
 static struct gpiomux_setting wcnss_5wire_suspend_cfg = {
 	.func = GPIOMUX_FUNC_GPIO,
 	.drv  = GPIOMUX_DRV_2MA,
@@ -411,6 +417,12 @@
 			[GPIOMUX_SUSPENDED] = &gpio_spi_config,
 		},
 	},
+	{
+		.gpio      = 81,		/* EPM enable */
+		.settings = {
+			[GPIOMUX_SUSPENDED] = &gpio_epm_config,
+		},
+	},
 };
 
 static struct msm_gpiomux_config msm8974_slimbus_config[] __initdata = {
diff --git a/arch/arm/mach-msm/board-8974.c b/arch/arm/mach-msm/board-8974.c
index ca8f68a..2f6d3d0 100644
--- a/arch/arm/mach-msm/board-8974.c
+++ b/arch/arm/mach-msm/board-8974.c
@@ -48,6 +48,7 @@
 #include "spm.h"
 #include "modem_notifier.h"
 #include "lpm_resources.h"
+#include "platsmp.h"
 
 
 static struct memtype_reserve msm8974_reserve_table[] __initdata = {
@@ -312,11 +313,6 @@
 			"msm_sdcc.3", NULL),
 	OF_DEV_AUXDATA("qcom,msm-sdcc", 0xF98E4000, \
 			"msm_sdcc.4", NULL),
-	OF_DEV_AUXDATA("qcom,pil-q6v5-lpass",   0xFE200000, \
-			"pil-q6v5-lpass", NULL),
-	OF_DEV_AUXDATA("qcom,pil-q6v5-mss", 0xFC880000, "pil-q6v5-mss", NULL),
-	OF_DEV_AUXDATA("qcom,pil-pronto", 0xFB21B000, \
-			"pil_pronto", NULL),
 	OF_DEV_AUXDATA("arm,coresight-tmc", 0xFC322000, \
 			"coresight-tmc-etr", NULL),
 	OF_DEV_AUXDATA("arm,coresight-tpiu", 0xFC318000, \
@@ -397,4 +393,5 @@
 	.reserve = msm_8974_reserve,
 	.init_very_early = msm8974_init_very_early,
 	.restart = msm_restart,
+	.smp = &msm8974_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-9615.c b/arch/arm/mach-msm/board-9615.c
index 1dcd54f..2392f57 100644
--- a/arch/arm/mach-msm/board-9615.c
+++ b/arch/arm/mach-msm/board-9615.c
@@ -882,7 +882,9 @@
 	&msm_device_hsic_host,
 	&msm_device_usb_bam,
 	&msm_android_usb_device,
+#ifdef CONFIG_USB_CI13XXX_MSM_HSIC
 	&msm_android_usb_hsic_device,
+#endif
 	&msm9615_device_uart_gsbi4,
 	&msm9615_device_ext_2p95v_vreg,
 	&msm9615_device_ssbi_pmic1,
diff --git a/arch/arm/mach-msm/board-fsm9xxx.c b/arch/arm/mach-msm/board-fsm9xxx.c
index 1d6eb01..274b338 100644
--- a/arch/arm/mach-msm/board-fsm9xxx.c
+++ b/arch/arm/mach-msm/board-fsm9xxx.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010-2012, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2011-2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -46,6 +46,7 @@
 #include <linux/msm_adc.h>
 #include <linux/m_adcproc.h>
 #include <linux/platform_data/qcom_crypto_device.h>
+#include <linux/msm_ion.h>
 
 #define PMIC_GPIO_INT		144
 #define PMIC_VREG_WLAN_LEVEL	2900
@@ -723,32 +724,32 @@
 
 static struct resource qcrypto_resources[] = {
 	[0] = {
-		.start = QCE_0_BASE,
-		.end = QCE_0_BASE + QCE_SIZE - 1,
+		.start = QCE_1_BASE,
+		.end = QCE_1_BASE + QCE_SIZE - 1,
 		.flags = IORESOURCE_MEM,
 	},
 	[1] = {
 		.name = "crypto_channels",
-		.start = DMOV_CE1_IN_CHAN,
-		.end = DMOV_CE1_OUT_CHAN,
+		.start = DMOV_CE2_IN_CHAN,
+		.end = DMOV_CE2_OUT_CHAN,
 		.flags = IORESOURCE_DMA,
 	},
 	[2] = {
 		.name = "crypto_crci_in",
-		.start = DMOV_CE1_IN_CRCI,
-		.end = DMOV_CE1_IN_CRCI,
+		.start = DMOV_CE2_IN_CRCI,
+		.end = DMOV_CE2_IN_CRCI,
 		.flags = IORESOURCE_DMA,
 	},
 	[3] = {
 		.name = "crypto_crci_out",
-		.start = DMOV_CE1_OUT_CRCI,
-		.end = DMOV_CE1_OUT_CRCI,
+		.start = DMOV_CE2_OUT_CRCI,
+		.end = DMOV_CE2_OUT_CRCI,
 		.flags = IORESOURCE_DMA,
 	},
 	[4] = {
 		.name = "crypto_crci_hash",
-		.start = DMOV_CE1_HASH_CRCI,
-		.end = DMOV_CE1_HASH_CRCI,
+		.start = DMOV_CE2_HASH_CRCI,
+		.end = DMOV_CE2_HASH_CRCI,
 		.flags = IORESOURCE_DMA,
 	},
 };
@@ -774,57 +775,6 @@
 
 static struct resource qcedev_resources[] = {
 	[0] = {
-		.start = QCE_0_BASE,
-		.end = QCE_0_BASE + QCE_SIZE - 1,
-		.flags = IORESOURCE_MEM,
-	},
-	[1] = {
-		.name = "crypto_channels",
-		.start = DMOV_CE1_IN_CHAN,
-		.end = DMOV_CE1_OUT_CHAN,
-		.flags = IORESOURCE_DMA,
-	},
-	[2] = {
-		.name = "crypto_crci_in",
-		.start = DMOV_CE1_IN_CRCI,
-		.end = DMOV_CE1_IN_CRCI,
-		.flags = IORESOURCE_DMA,
-	},
-	[3] = {
-		.name = "crypto_crci_out",
-		.start = DMOV_CE1_OUT_CRCI,
-		.end = DMOV_CE1_OUT_CRCI,
-		.flags = IORESOURCE_DMA,
-	},
-	[4] = {
-		.name = "crypto_crci_hash",
-		.start = DMOV_CE1_HASH_CRCI,
-		.end = DMOV_CE1_HASH_CRCI,
-		.flags = IORESOURCE_DMA,
-	},
-};
-
-static struct msm_ce_hw_support qcedev_ce_hw_suppport = {
-	.ce_shared = QCE_NO_CE_SHARED,
-	.shared_ce_resource = QCE_NO_SHARE_CE_RESOURCE,
-	.hw_key_support = QCE_NO_HW_KEY_SUPPORT,
-	.sha_hmac = QCE_NO_SHA_HMAC_SUPPORT,
-	.bus_scale_table = NULL,
-};
-
-static struct platform_device qcedev_device = {
-	.name		= "qce",
-	.id		= 0,
-	.num_resources	= ARRAY_SIZE(qcedev_resources),
-	.resource	= qcedev_resources,
-	.dev		= {
-		.coherent_dma_mask = DMA_BIT_MASK(32),
-		.platform_data = &qcedev_ce_hw_suppport,
-	},
-};
-
-static struct resource ota_qcrypto_resources[] = {
-	[0] = {
 		.start = QCE_1_BASE,
 		.end = QCE_1_BASE + QCE_SIZE - 1,
 		.flags = IORESOURCE_MEM,
@@ -855,6 +805,57 @@
 	},
 };
 
+static struct msm_ce_hw_support qcedev_ce_hw_suppport = {
+	.ce_shared = QCE_NO_CE_SHARED,
+	.shared_ce_resource = QCE_NO_SHARE_CE_RESOURCE,
+	.hw_key_support = QCE_NO_HW_KEY_SUPPORT,
+	.sha_hmac = QCE_NO_SHA_HMAC_SUPPORT,
+	.bus_scale_table = NULL,
+};
+
+static struct platform_device qcedev_device = {
+	.name		= "qce",
+	.id		= 0,
+	.num_resources	= ARRAY_SIZE(qcedev_resources),
+	.resource	= qcedev_resources,
+	.dev		= {
+		.coherent_dma_mask = DMA_BIT_MASK(32),
+		.platform_data = &qcedev_ce_hw_suppport,
+	},
+};
+
+static struct resource ota_qcrypto_resources[] = {
+	[0] = {
+		.start = QCE_2_BASE,
+		.end = QCE_2_BASE + QCE_SIZE - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	[1] = {
+		.name = "crypto_channels",
+		.start = DMOV_CE3_IN_CHAN,
+		.end = DMOV_CE3_OUT_CHAN,
+		.flags = IORESOURCE_DMA,
+	},
+	[2] = {
+		.name = "crypto_crci_in",
+		.start = DMOV_CE3_IN_CRCI,
+		.end = DMOV_CE3_IN_CRCI,
+		.flags = IORESOURCE_DMA,
+	},
+	[3] = {
+		.name = "crypto_crci_out",
+		.start = DMOV_CE3_OUT_CRCI,
+		.end = DMOV_CE3_OUT_CRCI,
+		.flags = IORESOURCE_DMA,
+	},
+	[4] = {
+		.name = "crypto_crci_hash",
+		.start = DMOV_CE3_HASH_DONE_CRCI,
+		.end = DMOV_CE3_HASH_DONE_CRCI,
+		.flags = IORESOURCE_DMA,
+	},
+};
+
 struct platform_device ota_qcrypto_device = {
 	.name		= "qcota",
 	.id		= 0,
@@ -870,6 +871,27 @@
 	.id		= -1,
 };
 
+struct ion_platform_heap msm_ion_heaps[] = {
+	{
+		.id = ION_SYSTEM_HEAP_ID,
+		.type = ION_HEAP_TYPE_SYSTEM_CONTIG,
+		.name = "kmalloc",
+	},
+};
+
+static struct ion_platform_data msm_ion_pdata = {
+	.nr = 1,
+	.heaps = msm_ion_heaps,
+};
+
+static struct platform_device msm_ion_device = {
+	.name = "ion-msm",
+	.id = 1,
+	.dev = {
+		.platform_data = &msm_ion_pdata,
+	},
+};
+
 /*
  * Devices
  */
@@ -905,6 +927,7 @@
 	&ota_qcrypto_device,
 	&fsm_xo_device,
 	&fsm9xxx_device_watchdog,
+	&msm_ion_device,
 };
 
 static void __init fsm9xxx_init_irq(void)
diff --git a/arch/arm/mach-msm/board-msm7627a-io.c b/arch/arm/mach-msm/board-msm7627a-io.c
index 8c52456..4ec1d33 100644
--- a/arch/arm/mach-msm/board-msm7627a-io.c
+++ b/arch/arm/mach-msm/board-msm7627a-io.c
@@ -887,7 +887,9 @@
 		ft5x06_touchpad_setup();
 	}
 
-	/* headset */
+	/* headset and power key*/
+	/* ignore end key as this target doesn't need it */
+	hs_platform_data.ignore_end_key = true;
 	platform_device_register(&hs_pdev);
 
 	/* vibrator */
diff --git a/arch/arm/mach-msm/board-msm7627a-storage.c b/arch/arm/mach-msm/board-msm7627a-storage.c
index 07ff389..5351d41 100644
--- a/arch/arm/mach-msm/board-msm7627a-storage.c
+++ b/arch/arm/mach-msm/board-msm7627a-storage.c
@@ -369,6 +369,14 @@
 	if (!(machine_is_msm7627a_qrd3() || machine_is_msm8625_qrd7())) {
 		if (mmc_regulator_init(3, "emmc", 3000000))
 			return;
+		/*
+		 * On 7x25A FFA data CRC errors are seen, which are
+		 * probably due to the proximity of SIM card and eMMC.
+		 * Hence, reducing the clock to 24.7Mhz from 49Mhz.
+		 */
+		if (machine_is_msm7625a_ffa())
+			sdc3_plat_data.msmsdcc_fmax =
+				sdc3_plat_data.msmsdcc_fmid;
 		msm_add_sdcc(3, &sdc3_plat_data);
 	}
 #endif
diff --git a/arch/arm/mach-msm/board-msm7x27a.c b/arch/arm/mach-msm/board-msm7x27a.c
index 9fd5218..3104aac 100644
--- a/arch/arm/mach-msm/board-msm7x27a.c
+++ b/arch/arm/mach-msm/board-msm7x27a.c
@@ -58,6 +58,7 @@
 #include <mach/socinfo.h>
 #include "pm-boot.h"
 #include "board-msm7627a.h"
+#include "platsmp.h"
 
 #define RESERVE_KERNEL_EBI1_SIZE	0x3A000
 #define MSM_RESERVE_AUDIO_SIZE	0x1F4000
@@ -403,7 +404,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(0, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -422,7 +423,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(1, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -441,7 +442,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(2, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -460,7 +461,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(3, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -954,7 +955,7 @@
 	}
 	rpc_adsp_pdev->prog = ADSP_RPC_PROG;
 
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		rpc_adsp_pdev->pdev = msm8625_device_adsp;
 	else
 		rpc_adsp_pdev->pdev = msm_adsp_device;
@@ -1031,7 +1032,7 @@
 {
 	msm7x27a_cfg_uart2dm_serial();
 	msm_uart_dm1_pdata.wakeup_irq = gpio_to_irq(UART1DM_RX_GPIO);
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		msm8625_device_uart_dm1.dev.platform_data =
 			&msm_uart_dm1_pdata;
 	else
@@ -1040,7 +1041,7 @@
 
 static void __init msm7x27a_otg_gadget(void)
 {
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		msm_otg_pdata.swfi_latency =
 		msm8625_pm_data[MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT].latency;
 		msm8625_device_otg.dev.platform_data = &msm_otg_pdata;
@@ -1080,7 +1081,7 @@
 	/* Initialize regulators first so that other devices can use them */
 	msm7x27a_init_regulators();
 	msm_adsp_add_pdev();
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		msm8625_device_i2c_init();
 	else
 		msm7x27a_device_i2c_init();
@@ -1173,6 +1174,7 @@
 	.init_machine   = msm8625_rumi3_init,
 	.timer          = &msm_timer,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
 MACHINE_START(MSM8625_SURF, "QCT MSM8625 SURF")
 	.atag_offset    = 0x100,
@@ -1183,6 +1185,7 @@
 	.timer          = &msm_timer,
 	.init_early     = msm7x2x_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
 MACHINE_START(MSM8625_FFA, "QCT MSM8625 FFA")
 	.atag_offset    = 0x100,
@@ -1193,4 +1196,5 @@
 	.timer          = &msm_timer,
 	.init_early     = msm7x2x_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-msm8x60.c b/arch/arm/mach-msm/board-msm8x60.c
index 08e6a0d..d4205bd 100644
--- a/arch/arm/mach-msm/board-msm8x60.c
+++ b/arch/arm/mach-msm/board-msm8x60.c
@@ -103,6 +103,7 @@
 #include "clock.h"
 #include "pm-boot.h"
 #include "board-storage-common-a.h"
+#include "platsmp.h"
 
 #include <linux/msm_ion.h>
 #include <mach/ion.h>
@@ -10506,6 +10507,7 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8X60_FFA, "QCT MSM8X60 FFA")
@@ -10517,6 +10519,7 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8X60_FLUID, "QCT MSM8X60 FLUID")
@@ -10528,6 +10531,7 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8X60_FUSION, "QCT MSM8X60 FUSION SURF")
@@ -10539,6 +10543,7 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8X60_FUSN_FFA, "QCT MSM8X60 FUSION FFA")
@@ -10550,6 +10555,7 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
 
 MACHINE_START(MSM8X60_DRAGON, "QCT MSM8X60 DRAGON")
@@ -10561,4 +10567,5 @@
 	.timer = &msm_timer,
 	.init_early = msm8x60_charm_init_early,
 	.restart = msm_restart,
+	.smp = &scorpion_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/board-qrd7627a.c b/arch/arm/mach-msm/board-qrd7627a.c
index 47a3120..ab2f1c3 100644
--- a/arch/arm/mach-msm/board-qrd7627a.c
+++ b/arch/arm/mach-msm/board-qrd7627a.c
@@ -62,6 +62,7 @@
 #include "pm-boot.h"
 #include "board-msm7x27a-regulator.h"
 #include "board-msm7627a.h"
+#include "platsmp.h"
 
 #define RESERVE_KERNEL_EBI1_SIZE	0x3A000
 #define MSM_RESERVE_AUDIO_SIZE	0x1F4000
@@ -135,24 +136,24 @@
 	.msm_i2c_config_gpio	= gsbi_qup_i2c_gpio_config,
 };
 
-static struct msm_gpio i2c_gpio_config[] = {
+static struct msm_gpio msm8625q_i2c_gpio_config[] = {
 	{ GPIO_CFG(39, 0, GPIO_CFG_OUTPUT, GPIO_CFG_NO_PULL, GPIO_CFG_8MA),
 		"qup_scl" },
 	{ GPIO_CFG(36, 0, GPIO_CFG_OUTPUT, GPIO_CFG_NO_PULL, GPIO_CFG_8MA),
 		"qup_sda" },
 };
 
-static struct i2c_gpio_platform_data i2c_gpio_pdata = {
+static struct i2c_gpio_platform_data msm8625q_i2c_gpio_pdata = {
 	.scl_pin = 39,
 	.sda_pin = 36,
 	.udelay = 5, /* 100 Khz */
 };
 
-static struct platform_device msm_i2c_gpio = {
+static struct platform_device msm8625q_i2c_gpio = {
 	.name	= "i2c-gpio",
 	.id	= 2,
 	.dev	= {
-		.platform_data = &i2c_gpio_pdata,
+		.platform_data = &msm8625q_i2c_gpio_pdata,
 	}
 };
 
@@ -382,7 +383,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(0, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -401,7 +402,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(1, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -420,7 +421,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(2, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -439,7 +440,7 @@
 					.idle_enabled = 0,
 					.suspend_enabled = 0,
 					.latency = 500,
-					.residency = 6000,
+					.residency = 500,
 	},
 
 	[MSM_PM_MODE(3, MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT)] = {
@@ -667,6 +668,10 @@
 /* Regulator configuration for the NCP6335D buck */
 struct regulator_consumer_supply ncp6335d_consumer_supplies[] = {
 	REGULATOR_SUPPLY("ncp6335d", NULL),
+	/* TO DO: NULL entry needs to be fixed once
+	 * we fix the cross-dependencies.
+	*/
+	REGULATOR_SUPPLY("vddx_cx", NULL),
 };
 
 static struct regulator_init_data ncp6335d_init_data = {
@@ -753,7 +758,6 @@
 	&msm8625_device_smd,
 	&msm8625_gsbi0_qup_i2c_device,
 	&msm8625_gsbi1_qup_i2c_device,
-	&msm_i2c_gpio,  /* TODO: Make this specific to 8625q */
 	&msm8625_device_uart1,
 	&msm8625_device_uart_dm1,
 	&msm8625_device_otg,
@@ -933,7 +937,7 @@
 	}
 	rpc_adsp_pdev->prog = ADSP_RPC_PROG;
 
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		rpc_adsp_pdev->pdev = msm8625_device_adsp;
 	else
 		rpc_adsp_pdev->pdev = msm_adsp_device;
@@ -958,13 +962,18 @@
 					= &msm_gsbi0_qup_i2c_pdata;
 	msm8625_gsbi1_qup_i2c_device.dev.platform_data
 					= &msm_gsbi1_qup_i2c_pdata;
-	if (machine_is_qrd_skud_prime()) {
-		for (i = 0 ; i < ARRAY_SIZE(i2c_gpio_config); i++) {
-			rc = gpio_tlmm_config(i2c_gpio_config[i].gpio_cfg,
+	if (machine_is_qrd_skud_prime() || cpu_is_msm8625q()) {
+		for (i = 0 ; i < ARRAY_SIZE(msm8625q_i2c_gpio_config); i++) {
+			rc = gpio_tlmm_config(
+					msm8625q_i2c_gpio_config[i].gpio_cfg,
 					GPIO_CFG_ENABLE);
 			if (rc)
 				pr_err("I2C-gpio tlmm config failed\n");
 		}
+		rc = platform_device_register(&msm8625q_i2c_gpio);
+		if (rc)
+			pr_err("%s: could not register i2c-gpio device: %d\n",
+						__func__, rc);
 	}
 }
 
@@ -1041,7 +1050,7 @@
 static void __init qrd7627a_uart1dm_config(void)
 {
 	msm_uart_dm1_pdata.wakeup_irq = gpio_to_irq(UART1DM_RX_GPIO);
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		msm8625_device_uart_dm1.dev.platform_data =
 			&msm_uart_dm1_pdata;
 	else
@@ -1050,7 +1059,7 @@
 
 static void __init qrd7627a_otg_gadget(void)
 {
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		msm_otg_pdata.swfi_latency = msm8625_pm_data
 		[MSM_PM_SLEEP_MODE_WAIT_FOR_INTERRUPT].latency;
 		msm8625_device_otg.dev.platform_data = &msm_otg_pdata;
@@ -1069,7 +1078,7 @@
 static void __init msm_pm_init(void)
 {
 
-	if (!cpu_is_msm8625()) {
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q()) {
 		msm_pm_set_platform_data(msm7627a_pm_data,
 				ARRAY_SIZE(msm7627a_pm_data));
 		BUG_ON(msm_pm_boot_init(&msm_pm_boot_pdata));
@@ -1088,7 +1097,7 @@
 	msm7627a_init_regulators();
 	msmqrd_adsp_add_pdev();
 
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		msm8625_device_i2c_init();
 	else
 		msm7627a_device_i2c_init();
@@ -1172,6 +1181,7 @@
 	.timer		= &msm_timer,
 	.init_early	= qrd7627a_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
 MACHINE_START(MSM8625_QRD7, "QRD MSM8625 QRD7")
 	.atag_offset	= 0x100,
@@ -1182,6 +1192,7 @@
 	.timer		= &msm_timer,
 	.init_early	= qrd7627a_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
 MACHINE_START(MSM8625_EVT, "QRD MSM8625 EVT")
 	.atag_offset	= 0x100,
@@ -1192,6 +1203,7 @@
 	.timer		= &msm_timer,
 	.init_early	= qrd7627a_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
 MACHINE_START(QRD_SKUD_PRIME, "QRD MSM8625 SKUD PRIME")
 	.atag_offset	= 0x100,
@@ -1202,4 +1214,5 @@
 	.timer		= &msm_timer,
 	.init_early	= qrd7627a_init_early,
 	.handle_irq	= gic_handle_irq,
+	.smp		= &msm8625_smp_ops,
 MACHINE_END
diff --git a/arch/arm/mach-msm/cache_erp.c b/arch/arm/mach-msm/cache_erp.c
index 8a73c84..6b8f58b 100644
--- a/arch/arm/mach-msm/cache_erp.c
+++ b/arch/arm/mach-msm/cache_erp.c
@@ -20,6 +20,7 @@
 #include <linux/io.h>
 #include <mach/msm-krait-l2-accessors.h>
 #include <mach/msm_iomap.h>
+#include <mach/socinfo.h>
 #include <asm/cputype.h>
 #include "acpuclock.h"
 
@@ -32,6 +33,8 @@
 #define CESR_TLBMH		BIT(16)
 #define CESR_I_MASK		0x000000CC
 
+#define CESR_VALID_MASK		0x000100FF
+
 /* Print a message for everything but TLB MH events */
 #define CESR_PRINT_MASK		0x000000FF
 
@@ -64,6 +67,12 @@
 #define ERP_L1_ERR(a) do { } while (0)
 #endif
 
+#ifdef CONFIG_MSM_L1_RECOV_ERR_PANIC
+#define ERP_L1_RECOV_ERR(a) panic(a)
+#else
+#define ERP_L1_RECOV_ERR(a) do { } while (0)
+#endif
+
 #ifdef CONFIG_MSM_L2_ERP_PORT_PANIC
 #define ERP_PORT_ERR(a) panic(a)
 #else
@@ -319,8 +328,13 @@
 	/* Clear the interrupt bits we processed */
 	write_cesr(cesr);
 
-	if (print_regs)
-		ERP_L1_ERR("L1 cache error detected");
+	if (print_regs) {
+		if ((cesr & (~CESR_I_MASK & CESR_VALID_MASK)) ||
+		    cpu_is_krait_v1() || cpu_is_krait_v2())
+			ERP_L1_ERR("L1 nonrecoverable cache error detected");
+		else
+			ERP_L1_RECOV_ERR("L1 recoverable error detected\n");
+	}
 
 	return IRQ_HANDLED;
 }
diff --git a/arch/arm/mach-msm/clock-8910.c b/arch/arm/mach-msm/clock-8910.c
new file mode 100644
index 0000000..c5541b4
--- /dev/null
+++ b/arch/arm/mach-msm/clock-8910.c
@@ -0,0 +1,3443 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/ctype.h>
+#include <linux/io.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/clk.h>
+#include <linux/iopoll.h>
+
+#include <mach/rpm-regulator-smd.h>
+#include <mach/socinfo.h>
+#include <mach/rpm-smd.h>
+
+#include "clock-local2.h"
+#include "clock-pll.h"
+#include "clock-rpm.h"
+#include "clock-voter.h"
+#include "clock.h"
+
+enum {
+	GCC_BASE,
+	MMSS_BASE,
+	LPASS_BASE,
+	APCS_BASE,
+	N_BASES,
+};
+
+static void __iomem *virt_bases[N_BASES];
+
+#define GCC_REG_BASE(x) (void __iomem *)(virt_bases[GCC_BASE] + (x))
+#define MMSS_REG_BASE(x) (void __iomem *)(virt_bases[MMSS_BASE] + (x))
+#define LPASS_REG_BASE(x) (void __iomem *)(virt_bases[LPASS_BASE] + (x))
+#define APCS_REG_BASE(x) (void __iomem *)(virt_bases[APCS_BASE] + (x))
+
+#define                     GPLL0_MODE	    0x0000
+#define                    GPLL0_L_VAL	    0x0004
+#define                    GPLL0_M_VAL	    0x0008
+#define                    GPLL0_N_VAL	    0x000C
+#define                 GPLL0_USER_CTL	    0x0010
+#define                   GPLL0_STATUS	    0x001C
+#define                     GPLL2_MODE	    0x0080
+#define                    GPLL2_L_VAL	    0x0084
+#define                    GPLL2_M_VAL	    0x0088
+#define                    GPLL2_N_VAL	    0x008C
+#define                 GPLL2_USER_CTL	    0x0090
+#define                   GPLL2_STATUS	    0x009C
+#define                 CONFIG_NOC_BCR	    0x0140
+#define                       MMSS_BCR	    0x0240
+#define          MMSS_NOC_CFG_AHB_CBCR	    0x024C
+#define               MSS_CFG_AHB_CBCR	    0x0280
+#define           MSS_Q6_BIMC_AXI_CBCR	    0x0284
+#define                     USB_HS_BCR	    0x0480
+#define             USB_HS_SYSTEM_CBCR	    0x0484
+#define                USB_HS_AHB_CBCR	    0x0488
+#define         USB_HS_SYSTEM_CMD_RCGR	    0x0490
+#define                  USB2A_PHY_BCR	    0x04A8
+#define           USB2A_PHY_SLEEP_CBCR	    0x04AC
+#define                      SDCC1_BCR	    0x04C0
+#define            SDCC1_APPS_CMD_RCGR	    0x04D0
+#define                SDCC1_APPS_CBCR	    0x04C4
+#define                 SDCC1_AHB_CBCR	    0x04C8
+#define                      SDCC2_BCR	    0x0500
+#define            SDCC2_APPS_CMD_RCGR	    0x0510
+#define                SDCC2_APPS_CBCR	    0x0504
+#define                 SDCC2_AHB_CBCR	    0x0508
+#define                      BLSP1_BCR	    0x05C0
+#define                 BLSP1_AHB_CBCR	    0x05C4
+#define                 BLSP1_QUP1_BCR	    0x0640
+#define       BLSP1_QUP1_SPI_APPS_CBCR	    0x0644
+#define       BLSP1_QUP1_I2C_APPS_CBCR	    0x0648
+#define   BLSP1_QUP1_SPI_APPS_CMD_RCGR	    0x064C
+#define                BLSP1_UART1_BCR	    0x0680
+#define          BLSP1_UART1_APPS_CBCR	    0x0684
+#define           BLSP1_UART1_SIM_CBCR	    0x0688
+#define      BLSP1_UART1_APPS_CMD_RCGR	    0x068C
+#define                 BLSP1_QUP2_BCR	    0x06C0
+#define       BLSP1_QUP2_SPI_APPS_CBCR	    0x06C4
+#define       BLSP1_QUP2_I2C_APPS_CBCR	    0x06C8
+#define   BLSP1_QUP2_SPI_APPS_CMD_RCGR	    0x06CC
+#define                BLSP1_UART2_BCR	    0x0700
+#define          BLSP1_UART2_APPS_CBCR	    0x0704
+#define           BLSP1_UART2_SIM_CBCR	    0x0708
+#define      BLSP1_UART2_APPS_CMD_RCGR	    0x070C
+#define                 BLSP1_QUP3_BCR	    0x0740
+#define       BLSP1_QUP3_SPI_APPS_CBCR	    0x0744
+#define       BLSP1_QUP3_I2C_APPS_CBCR	    0x0748
+#define   BLSP1_QUP3_SPI_APPS_CMD_RCGR	    0x074C
+#define                BLSP1_UART3_BCR	    0x0780
+#define          BLSP1_UART3_APPS_CBCR	    0x0784
+#define           BLSP1_UART3_SIM_CBCR	    0x0788
+#define      BLSP1_UART3_APPS_CMD_RCGR	    0x078C
+#define                 BLSP1_QUP4_BCR	    0x07C0
+#define       BLSP1_QUP4_SPI_APPS_CBCR	    0x07C4
+#define       BLSP1_QUP4_I2C_APPS_CBCR	    0x07C8
+#define   BLSP1_QUP4_SPI_APPS_CMD_RCGR	    0x07CC
+#define                BLSP1_UART4_BCR	    0x0800
+#define          BLSP1_UART4_APPS_CBCR	    0x0804
+#define           BLSP1_UART4_SIM_CBCR	    0x0808
+#define      BLSP1_UART4_APPS_CMD_RCGR	    0x080C
+#define                 BLSP1_QUP5_BCR	    0x0840
+#define       BLSP1_QUP5_SPI_APPS_CBCR	    0x0844
+#define       BLSP1_QUP5_I2C_APPS_CBCR	    0x0848
+#define   BLSP1_QUP5_SPI_APPS_CMD_RCGR	    0x084C
+#define                BLSP1_UART5_BCR	    0x0880
+#define          BLSP1_UART5_APPS_CBCR	    0x0884
+#define           BLSP1_UART5_SIM_CBCR	    0x0888
+#define      BLSP1_UART5_APPS_CMD_RCGR	    0x088C
+#define                 BLSP1_QUP6_BCR	    0x08C0
+#define       BLSP1_QUP6_SPI_APPS_CBCR	    0x08C4
+#define       BLSP1_QUP6_I2C_APPS_CBCR	    0x08C8
+#define   BLSP1_QUP6_SPI_APPS_CMD_RCGR	    0x08CC
+#define                BLSP1_UART6_BCR	    0x0900
+#define          BLSP1_UART6_APPS_CBCR	    0x0904
+#define           BLSP1_UART6_SIM_CBCR	    0x0908
+#define      BLSP1_UART6_APPS_CMD_RCGR	    0x090C
+#define                        PDM_BCR	    0x0CC0
+#define                   PDM_AHB_CBCR	    0x0CC4
+#define                      PDM2_CBCR	    0x0CCC
+#define                  PDM2_CMD_RCGR	    0x0CD0
+#define                       PRNG_BCR	    0x0D00
+#define                  PRNG_AHB_CBCR	    0x0D04
+#define                   BOOT_ROM_BCR	    0x0E00
+#define              BOOT_ROM_AHB_CBCR	    0x0E04
+#define                        CE1_BCR	    0x1040
+#define                   CE1_CMD_RCGR	    0x1050
+#define                       CE1_CBCR	    0x1044
+#define                   CE1_AXI_CBCR	    0x1048
+#define                   CE1_AHB_CBCR	    0x104C
+#define            COPSS_SMMU_AHB_CBCR      0x015C
+#define             LPSS_SMMU_AHB_CBCR      0x0158
+#define              LPASS_Q6_AXI_CBCR	    0x11C0
+#define             APCS_GPLL_ENA_VOTE	    0x1480
+#define     APCS_CLOCK_BRANCH_ENA_VOTE	    0x1484
+#define      APCS_CLOCK_SLEEP_ENA_VOTE	    0x1488
+#define                       GP1_CBCR	    0x1900
+#define                   GP1_CMD_RCGR	    0x1904
+#define                       GP2_CBCR	    0x1940
+#define                   GP2_CMD_RCGR	    0x1944
+#define                       GP3_CBCR	    0x1980
+#define                   GP3_CMD_RCGR	    0x1984
+#define                        XO_CBCR	    0x0034
+
+#define                MMPLL0_PLL_MODE	    0x0000
+#define               MMPLL0_PLL_L_VAL	    0x0004
+#define               MMPLL0_PLL_M_VAL	    0x0008
+#define               MMPLL0_PLL_N_VAL	    0x000C
+#define            MMPLL0_PLL_USER_CTL	    0x0010
+#define              MMPLL0_PLL_STATUS	    0x001C
+#define         MMSS_PLL_VOTE_APCS_REG      0x0100
+#define                MMPLL1_PLL_MODE	    0x4100
+#define               MMPLL1_PLL_L_VAL	    0x4104
+#define               MMPLL1_PLL_M_VAL	    0x4108
+#define               MMPLL1_PLL_N_VAL	    0x410C
+#define            MMPLL1_PLL_USER_CTL	    0x4110
+#define              MMPLL1_PLL_STATUS	    0x411C
+#define              DSI_PCLK_CMD_RCGR	    0x2000
+#define                   DSI_CMD_RCGR	    0x2020
+#define             MDP_VSYNC_CMD_RCGR	    0x2080
+#define              DSI_BYTE_CMD_RCGR	    0x2120
+#define               DSI_ESC_CMD_RCGR	    0x2160
+#define                        DSI_BCR	    0x2200
+#define                   DSI_BYTE_BCR	    0x2204
+#define                    DSI_ESC_BCR	    0x2208
+#define                    DSI_AHB_BCR	    0x220C
+#define                   DSI_PCLK_BCR	    0x2214
+#define                   MDP_LCDC_BCR	    0x2218
+#define                    MDP_DSI_BCR	    0x221C
+#define                  MDP_VSYNC_BCR	    0x2220
+#define                    MDP_AXI_BCR	    0x2224
+#define                    MDP_AHB_BCR	    0x2228
+#define                   MDP_AXI_CBCR	    0x2314
+#define                 MDP_VSYNC_CBCR	    0x231C
+#define                   MDP_AHB_CBCR	    0x2318
+#define                  DSI_PCLK_CBCR	    0x233C
+#define                GMEM_GFX3D_CBCR      0x4038
+#define                  MDP_LCDC_CBCR	    0x2340
+#define                   MDP_DSI_CBCR	    0x2320
+#define                       DSI_CBCR	    0x2324
+#define                  DSI_BYTE_CBCR	    0x2328
+#define                   DSI_ESC_CBCR	    0x232C
+#define                   DSI_AHB_CBCR	    0x2330
+#define          CSI0PHYTIMER_CMD_RCGR	    0x3000
+#define               CSI0PHYTIMER_BCR	    0x3020
+#define              CSI0PHYTIMER_CBCR	    0x3024
+#define          CSI1PHYTIMER_CMD_RCGR	    0x3030
+#define               CSI1PHYTIMER_BCR	    0x3050
+#define              CSI1PHYTIMER_CBCR	    0x3054
+#define                  CSI0_CMD_RCGR	    0x3090
+#define                       CSI0_BCR	    0x30B0
+#define                      CSI0_CBCR	    0x30B4
+#define                    CSI_AHB_BCR	    0x30B8
+#define                   CSI_AHB_CBCR	    0x30BC
+#define                    CSI0PHY_BCR	    0x30C0
+#define                   CSI0PHY_CBCR	    0x30C4
+#define                    CSI0RDI_BCR	    0x30D0
+#define                   CSI0RDI_CBCR	    0x30D4
+#define                    CSI0PIX_BCR	    0x30E0
+#define                   CSI0PIX_CBCR	    0x30E4
+#define                  CSI1_CMD_RCGR	    0x3100
+#define                       CSI1_BCR	    0x3120
+#define                      CSI1_CBCR	    0x3124
+#define                    CSI1PHY_BCR	    0x3130
+#define                   CSI1PHY_CBCR	    0x3134
+#define                    CSI1RDI_BCR	    0x3140
+#define                   CSI1RDI_CBCR	    0x3144
+#define                    CSI1PIX_BCR	    0x3150
+#define                   CSI1PIX_CBCR	    0x3154
+#define                 MCLK0_CMD_RCGR	    0x3360
+#define                      MCLK0_BCR	    0x3380
+#define                     MCLK0_CBCR	    0x3384
+#define                 MCLK1_CMD_RCGR	    0x3390
+#define                      MCLK1_BCR	    0x33B0
+#define                     MCLK1_CBCR	    0x33B4
+#define                   VFE_CMD_RCGR	    0x3600
+#define                        VFE_BCR	    0x36A0
+#define                    VFE_AHB_BCR	    0x36AC
+#define                    VFE_AXI_BCR	    0x36B0
+#define                       VFE_CBCR	    0x36A8
+#define                   VFE_AHB_CBCR	    0x36B8
+#define                   VFE_AXI_CBCR	    0x36BC
+#define                    CSI_VFE_BCR	    0x3700
+#define                   CSI_VFE_CBCR	    0x3704
+#define                 GFX3D_CMD_RCGR	    0x4000
+#define               OXILI_GFX3D_CBCR	    0x4028
+#define                OXILI_GFX3D_BCR	    0x4030
+#define                  OXILI_AHB_BCR	    0x4044
+#define                 OXILI_AHB_CBCR	    0x403C
+#define                   AHB_CMD_RCGR	    0x5000
+#define                 MMSSNOCAHB_BCR	    0x5020
+#define             MMSSNOCAHB_BTO_BCR	    0x5030
+#define              MMSS_MISC_AHB_BCR	    0x5034
+#define          MMSS_MMSSNOC_AHB_CBCR	    0x5024
+#define      MMSS_MMSSNOC_BTO_AHB_CBCR	    0x5028
+#define             MMSS_MISC_AHB_CBCR	    0x502C
+#define                   AXI_CMD_RCGR	    0x5040
+#define                 MMSSNOCAXI_BCR	    0x5060
+#define                MMSS_S0_AXI_BCR	    0x5068
+#define               MMSS_S0_AXI_CBCR	    0x5064
+#define          MMSS_MMSSNOC_AXI_CBCR	    0x506C
+#define                   BIMC_GFX_BCR	    0x5090
+#define                  BIMC_GFX_CBCR	    0x5094
+
+#define				AUDIO_CORE_GDSCR	    0x7000
+#define                                 SPDM_BCR	    0x1000
+#define                        LPAAUDIO_PLL_MODE	    0x0000
+#define                       LPAAUDIO_PLL_L_VAL	    0x0004
+#define                       LPAAUDIO_PLL_M_VAL	    0x0008
+#define                       LPAAUDIO_PLL_N_VAL	    0x000C
+#define                    LPAAUDIO_PLL_USER_CTL	    0x0010
+#define                      LPAAUDIO_PLL_STATUS	    0x001C
+#define                           LPAQ6_PLL_MODE	    0x1000
+#define                       LPAQ6_PLL_USER_CTL	    0x1010
+#define                         LPAQ6_PLL_STATUS	    0x101C
+#define                        LPA_PLL_VOTE_APPS            0x2000
+#define                  AUDIO_CORE_BCR_SLP_CBCR	    0x4004
+#define                        Q6SS_BCR_SLP_CBCR	    0x6004
+#define                  AUDIO_CORE_GDSC_XO_CBCR	    0x7004
+#define                AUDIO_CORE_LPAIF_DMA_CBCR	    0x9000
+#define                AUDIO_CORE_LPAIF_CSR_CBCR	    0x9004
+#define                      LPAIF_SPKR_CMD_RCGR	    0xA000
+#define     AUDIO_CORE_LPAIF_CODEC_SPKR_OSR_CBCR	    0xA014
+#define    AUDIO_CORE_LPAIF_CODEC_SPKR_IBIT_CBCR	    0xA018
+#define    AUDIO_CORE_LPAIF_CODEC_SPKR_EBIT_CBCR	    0xA01C
+#define                       LPAIF_PRI_CMD_RCGR	    0xB000
+#define            AUDIO_CORE_LPAIF_PRI_OSR_CBCR	    0xB014
+#define           AUDIO_CORE_LPAIF_PRI_IBIT_CBCR	    0xB018
+#define           AUDIO_CORE_LPAIF_PRI_EBIT_CBCR	    0xB01C
+#define                       LPAIF_SEC_CMD_RCGR	    0xC000
+#define            AUDIO_CORE_LPAIF_SEC_OSR_CBCR	    0xC014
+#define           AUDIO_CORE_LPAIF_SEC_IBIT_CBCR	    0xC018
+#define           AUDIO_CORE_LPAIF_SEC_EBIT_CBCR	    0xC01C
+#define                       LPAIF_TER_CMD_RCGR	    0xD000
+#define            AUDIO_CORE_LPAIF_TER_OSR_CBCR	    0xD014
+#define           AUDIO_CORE_LPAIF_TER_IBIT_CBCR	    0xD018
+#define           AUDIO_CORE_LPAIF_TER_EBIT_CBCR	    0xD01C
+#define                      LPAIF_QUAD_CMD_RCGR	    0xE000
+#define           AUDIO_CORE_LPAIF_QUAD_OSR_CBCR	    0xE014
+#define          AUDIO_CORE_LPAIF_QUAD_IBIT_CBCR	    0xE018
+#define          AUDIO_CORE_LPAIF_QUAD_EBIT_CBCR	    0xE01C
+#define                      LPAIF_PCM0_CMD_RCGR	    0xF000
+#define          AUDIO_CORE_LPAIF_PCM0_IBIT_CBCR	    0xF014
+#define          AUDIO_CORE_LPAIF_PCM0_EBIT_CBCR	    0xF018
+#define                      LPAIF_PCM1_CMD_RCGR	   0x10000
+#define          AUDIO_CORE_LPAIF_PCM1_IBIT_CBCR	   0x10014
+#define          AUDIO_CORE_LPAIF_PCM1_EBIT_CBCR	   0x10018
+#define                         SLIMBUS_CMD_RCGR           0x12000
+#define             AUDIO_CORE_SLIMBUS_CORE_CBCR           0x12014
+#define                     LPAIF_PCMOE_CMD_RCGR	   0x13000
+#define        AUDIO_CORE_LPAIF_PCM_DATA_OE_CBCR	   0x13014
+#define                          Q6CORE_CMD_RCGR	   0x14000
+#define                           SLEEP_CMD_RCGR	   0x15000
+#define                            SPDM_CMD_RCGR	   0x16000
+#define                  AUDIO_WRAPPER_SPDM_CBCR	   0x16014
+#define                              XO_CMD_RCGR	   0x17000
+#define                       AHBFABRIC_CMD_RCGR	   0x18000
+#define                      AUDIO_CORE_LPM_CBCR	   0x19000
+#define               AUDIO_CORE_AVSYNC_CSR_CBCR	   0x1A000
+#define                AUDIO_CORE_AVSYNC_XO_CBCR	   0x1A004
+#define             AUDIO_CORE_AVSYNC_BT_XO_CBCR	   0x1A008
+#define             AUDIO_CORE_AVSYNC_FM_XO_CBCR	   0x1A00C
+#define                 AUDIO_CORE_IXFABRIC_CBCR	   0x1B000
+#define               AUDIO_WRAPPER_EFABRIC_CBCR	   0x1B004
+#define                AUDIO_CORE_TCM_SLAVE_CBCR	   0x1C000
+#define                      AUDIO_CORE_CSR_CBCR	   0x1D000
+#define                      AUDIO_CORE_DML_CBCR	   0x1E000
+#define                   AUDIO_CORE_SYSNOC_CBCR	   0x1F000
+#define           AUDIO_WRAPPER_SYSNOC_SWAY_CBCR	   0x1F004
+#define                  AUDIO_CORE_TIMEOUT_CBCR	   0x20000
+#define               AUDIO_WRAPPER_TIMEOUT_CBCR	   0x20004
+#define                 AUDIO_CORE_SECURITY_CBCR	   0x21000
+#define              AUDIO_WRAPPER_SECURITY_CBCR	   0x21004
+#define                     Q6SS_AHB_LFABIF_CBCR	   0x22000
+#define                           Q6SS_AHBM_CBCR	   0x22004
+#define               AUDIO_WRAPPER_LCC_CSR_CBCR	   0x23000
+#define                    AUDIO_WRAPPER_BR_CBCR	   0x24000
+#define                  AUDIO_WRAPPER_SMEM_CBCR	   0x25000
+#define                             Q6SS_XO_CBCR	   0x26000
+#define                            Q6SS_SLP_CBCR	   0x26004
+#define                           LPASS_Q6SS_BCR           0x6000
+#define                AUDIO_WRAPPER_STM_XO_CBCR	   0x27000
+#define      AUDIO_CORE_IXFABRIC_SPDMTM_CSR_CBCR	   0x28000
+#define    AUDIO_WRAPPER_EFABRIC_SPDMTM_CSR_CBCR	   0x28004
+
+/* Mux source select values */
+#define        gcc_xo_source_val 0
+#define         gpll0_source_val 1
+#define           gnd_source_val 5
+#define     mmpll0_mm_source_val 1
+#define     mmpll1_mm_source_val 2
+#define      gpll0_mm_source_val 5
+#define     gcc_xo_mm_source_val 0
+#define        mm_gnd_source_val 6
+#define     cxo_lpass_source_val 0
+#define lpapll0_lpass_source_val 1
+#define   gpll0_lpass_source_val 5
+#define     dsipll_mm_source_val 1
+
+#define F(f, s, div, m, n) \
+	{ \
+		.freq_hz = (f), \
+		.src_clk = &s##_clk_src.c, \
+		.m_val = (m), \
+		.n_val = ~((n)-(m)) * !!(n), \
+		.d_val = ~(n),\
+		.div_src_val = BVAL(4, 0, (int)(2*(div) - 1)) \
+			| BVAL(10, 8, s##_source_val), \
+	}
+
+#define F_MM(f, s, div, m, n) \
+	{ \
+		.freq_hz = (f), \
+		.src_clk = &s##_clk_src.c, \
+		.m_val = (m), \
+		.n_val = ~((n)-(m)) * !!(n), \
+		.d_val = ~(n),\
+		.div_src_val = BVAL(4, 0, (int)(2*(div) - 1)) \
+			| BVAL(10, 8, s##_mm_source_val), \
+	}
+
+#define F_HDMI(f, s, div, m, n) \
+	{ \
+		.freq_hz = (f), \
+		.src_clk = &s##_clk_src, \
+		.m_val = (m), \
+		.n_val = ~((n)-(m)) * !!(n), \
+		.d_val = ~(n),\
+		.div_src_val = BVAL(4, 0, (int)(2*(div) - 1)) \
+			| BVAL(10, 8, s##_mm_source_val), \
+	}
+
+#define F_MDSS(f, s, div, m, n) \
+	{ \
+		.freq_hz = (f), \
+		.m_val = (m), \
+		.n_val = ~((n)-(m)) * !!(n), \
+		.d_val = ~(n),\
+		.div_src_val = BVAL(4, 0, (int)(2*(div) - 1)) \
+			| BVAL(10, 8, s##_mm_source_val), \
+	}
+
+#define F_LPASS(f, s, div, m, n) \
+	{ \
+		.freq_hz = (f), \
+		.src_clk = &s##_clk_src.c, \
+		.m_val = (m), \
+		.n_val = ~((n)-(m)) * !!(n), \
+		.d_val = ~(n),\
+		.div_src_val = BVAL(4, 0, (int)(2*(div) - 1)) \
+			| BVAL(10, 8, s##_lpass_source_val), \
+	}
+
+#define VDD_DIG_FMAX_MAP1(l1, f1) \
+	.vdd_class = &vdd_dig,			\
+	.fmax = (unsigned long[VDD_DIG_NUM]) {	\
+		[VDD_DIG_##l1] = (f1),		\
+	},					\
+	.num_fmax = VDD_DIG_NUM
+#define VDD_DIG_FMAX_MAP2(l1, f1, l2, f2) \
+	.vdd_class = &vdd_dig,			\
+	.fmax = (unsigned long[VDD_DIG_NUM]) {	\
+		[VDD_DIG_##l1] = (f1),		\
+		[VDD_DIG_##l2] = (f2),		\
+	},					\
+	.num_fmax = VDD_DIG_NUM
+#define VDD_DIG_FMAX_MAP3(l1, f1, l2, f2, l3, f3) \
+	.vdd_class = &vdd_dig,			\
+	.fmax = (unsigned long[VDD_DIG_NUM]) {	\
+		[VDD_DIG_##l1] = (f1),		\
+		[VDD_DIG_##l2] = (f2),		\
+		[VDD_DIG_##l3] = (f3),		\
+	},					\
+	.num_fmax = VDD_DIG_NUM
+
+enum vdd_dig_levels {
+	VDD_DIG_NONE,
+	VDD_DIG_LOW,
+	VDD_DIG_NOMINAL,
+	VDD_DIG_HIGH,
+	VDD_DIG_NUM
+};
+
+static const int vdd_corner[] = {
+	[VDD_DIG_NONE]	  = RPM_REGULATOR_CORNER_NONE,
+	[VDD_DIG_LOW]	  = RPM_REGULATOR_CORNER_SVS_SOC,
+	[VDD_DIG_NOMINAL] = RPM_REGULATOR_CORNER_NORMAL,
+	[VDD_DIG_HIGH]	  = RPM_REGULATOR_CORNER_SUPER_TURBO,
+};
+
+static struct rpm_regulator *vdd_dig_reg;
+
+static int set_vdd_dig(struct clk_vdd_class *vdd_class, int level)
+{
+	return rpm_regulator_set_voltage(vdd_dig_reg, vdd_corner[level],
+					RPM_REGULATOR_CORNER_SUPER_TURBO);
+}
+
+static DEFINE_VDD_CLASS(vdd_dig, set_vdd_dig, VDD_DIG_NUM);
+
+#define RPM_MISC_CLK_TYPE	0x306b6c63
+#define RPM_BUS_CLK_TYPE	0x316b6c63
+#define RPM_MEM_CLK_TYPE	0x326b6c63
+
+#define RPM_SMD_KEY_ENABLE	0x62616E45
+
+#define CXO_ID			0x0
+#define QDSS_ID			0x1
+#define RPM_SCALING_ENABLE_ID	0x2
+
+#define PNOC_ID		0x0
+#define SNOC_ID		0x1
+#define CNOC_ID		0x2
+#define MMSSNOC_AHB_ID  0x3
+
+#define BIMC_ID		0x0
+#define OXILI_ID	0x1
+#define OCMEM_ID	0x2
+
+#define D0_ID		 1
+#define D1_ID		 2
+#define A0_ID		 3
+#define A1_ID		 4
+#define A2_ID		 5
+#define DIFF_CLK_ID	 7
+#define DIV_CLK_ID	11
+
+DEFINE_CLK_RPM_SMD(pnoc_clk, pnoc_a_clk, RPM_BUS_CLK_TYPE, PNOC_ID, NULL);
+DEFINE_CLK_RPM_SMD(snoc_clk, snoc_a_clk, RPM_BUS_CLK_TYPE, SNOC_ID, NULL);
+DEFINE_CLK_RPM_SMD(cnoc_clk, cnoc_a_clk, RPM_BUS_CLK_TYPE, CNOC_ID, NULL);
+DEFINE_CLK_RPM_SMD(mmssnoc_ahb_clk, mmssnoc_ahb_a_clk, RPM_BUS_CLK_TYPE,
+			MMSSNOC_AHB_ID, NULL);
+
+DEFINE_CLK_RPM_SMD(bimc_clk, bimc_a_clk, RPM_MEM_CLK_TYPE, BIMC_ID, NULL);
+
+DEFINE_CLK_RPM_SMD_BRANCH(gcc_xo_clk_src, gcc_xo_a_clk_src,
+				RPM_MISC_CLK_TYPE, CXO_ID, 19200000);
+DEFINE_CLK_RPM_SMD_QDSS(qdss_clk, qdss_a_clk, RPM_MISC_CLK_TYPE, QDSS_ID);
+
+DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_d0, cxo_d0_a, D0_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_d1, cxo_d1_a, D1_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a0, cxo_a0_a, A0_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a1, cxo_a1_a, A1_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a2, cxo_a2_a, A2_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(div_clk, div_a_clk, DIV_CLK_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(diff_clk, diff_a_clk, DIFF_CLK_ID);
+
+DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_d0_pin, cxo_d0_a_pin, D0_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_d1_pin, cxo_d1_a_pin, D1_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_a0_pin, cxo_a0_a_pin, A0_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_a1_pin, cxo_a1_a_pin, A1_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_a2_pin, cxo_a2_a_pin, A2_ID);
+
+static DEFINE_CLK_VOTER(pnoc_msmbus_clk, &pnoc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(snoc_msmbus_clk, &snoc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(cnoc_msmbus_clk, &cnoc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(pnoc_msmbus_a_clk, &pnoc_a_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(snoc_msmbus_a_clk, &snoc_a_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(cnoc_msmbus_a_clk, &cnoc_a_clk.c, LONG_MAX);
+
+static DEFINE_CLK_VOTER(bimc_msmbus_clk, &bimc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(bimc_msmbus_a_clk, &bimc_a_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(bimc_acpu_a_clk, &bimc_a_clk.c, LONG_MAX);
+
+static DEFINE_CLK_VOTER(pnoc_sps_clk, &pnoc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(pnoc_iommu_clk, &pnoc_clk.c, LONG_MAX);
+static DEFINE_CLK_VOTER(pnoc_qseecom_clk, &pnoc_clk.c, LONG_MAX);
+
+static struct pll_vote_clk gpll0_clk_src = {
+	.en_reg = (void __iomem *)APCS_GPLL_ENA_VOTE,
+	.en_mask = BIT(0),
+	.status_reg = (void __iomem *)GPLL0_STATUS,
+	.status_mask = BIT(17),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.rate = 600000000,
+		.dbg_name = "gpll0_clk_src",
+		.ops = &clk_ops_pll_vote,
+		CLK_INIT(gpll0_clk_src.c),
+	},
+};
+
+static struct pll_vote_clk mmpll0_clk_src = {
+	.en_reg = (void __iomem *)MMSS_PLL_VOTE_APCS_REG,
+	.en_mask = BIT(0),
+	.status_reg = (void __iomem *)MMPLL0_PLL_STATUS,
+	.status_mask = BIT(17),
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "mmpll0_clk_src",
+		.rate = 800000000,
+		.ops = &clk_ops_pll_vote,
+		CLK_INIT(mmpll0_clk_src.c),
+	},
+};
+
+static struct pll_config_regs mmpll0_regs __initdata = {
+	.l_reg = (void __iomem *)MMPLL0_PLL_L_VAL,
+	.m_reg = (void __iomem *)MMPLL0_PLL_M_VAL,
+	.n_reg = (void __iomem *)MMPLL0_PLL_N_VAL,
+	.config_reg = (void __iomem *)MMPLL0_PLL_USER_CTL,
+	.mode_reg = (void __iomem *)MMPLL0_PLL_MODE,
+	.base = &virt_bases[MMSS_BASE],
+};
+
+static struct pll_clk mmpll1_clk_src = {
+	.mode_reg = (void __iomem *)MMPLL1_PLL_MODE,
+	.status_reg = (void __iomem *)MMPLL1_PLL_STATUS,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "mmpll1_clk_src",
+		.rate = 1200000000,
+		.ops = &clk_ops_local_pll,
+		CLK_INIT(mmpll1_clk_src.c),
+	},
+};
+
+static struct pll_config_regs mmpll1_regs __initdata = {
+	.l_reg = (void __iomem *)MMPLL1_PLL_L_VAL,
+	.m_reg = (void __iomem *)MMPLL1_PLL_M_VAL,
+	.n_reg = (void __iomem *)MMPLL1_PLL_N_VAL,
+	.config_reg = (void __iomem *)MMPLL1_PLL_USER_CTL,
+	.mode_reg = (void __iomem *)MMPLL1_PLL_MODE,
+	.base = &virt_bases[MMSS_BASE],
+};
+
+static struct pll_vote_clk lpapll0_clk_src = {
+	.en_reg = (void __iomem *)LPA_PLL_VOTE_APPS,
+	.en_mask = BIT(0),
+	.status_reg = (void __iomem *)LPAAUDIO_PLL_STATUS,
+	.status_mask = BIT(17),
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.rate = 491520000,
+		.dbg_name = "lpapll0_clk_src",
+		.ops = &clk_ops_pll_vote,
+		CLK_INIT(lpapll0_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_blsp1_qup1_6_spi_apps_clk[] = {
+	F(  960000, gcc_xo, 10, 1, 2),
+	F( 4800000, gcc_xo,  4, 0, 0),
+	F( 9600000, gcc_xo,  2, 0, 0),
+	F(15000000,  gpll0, 10, 1, 4),
+	F(19200000, gcc_xo,  1, 0, 0),
+	F(25000000,  gpll0, 12, 1, 2),
+	F(50000000,  gpll0, 12, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk blsp1_qup1_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP1_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup1_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup1_spi_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_qup2_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP2_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup2_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup2_spi_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_qup3_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP3_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup3_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup3_spi_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_qup4_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP4_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup4_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup4_spi_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_qup5_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP5_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup5_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup5_spi_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_qup6_spi_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_QUP6_SPI_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_qup1_6_spi_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_qup6_spi_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 25000000, NOMINAL, 50000000),
+		CLK_INIT(blsp1_qup6_spi_apps_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_blsp1_uart1_6_apps_clk[] = {
+	F( 3686400,  gpll0,    1,   96, 15625),
+	F( 7372800,  gpll0,    1,  192, 15625),
+	F(14745600,  gpll0,    1,  384, 15625),
+	F(16000000,  gpll0,    5,    2,    15),
+	F(19200000, gcc_xo,    1,    0,     0),
+	F(24000000,  gpll0,    5,    1,     5),
+	F(32000000,  gpll0,    1,    4,    75),
+	F(40000000,  gpll0,   15,    0,     0),
+	F(46400000,  gpll0,    1,   29,   375),
+	F(48000000,  gpll0, 12.5,    0,     0),
+	F(51200000,  gpll0,    1,   32,   375),
+	F(56000000,  gpll0,    1,    7,    75),
+	F(58982400,  gpll0,    1, 1536, 15625),
+	F(60000000,  gpll0,   10,    0,     0),
+	F_END,
+};
+
+static struct rcg_clk blsp1_uart1_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART1_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart1_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart1_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_uart2_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART2_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart2_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart2_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_uart3_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART3_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart3_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart3_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_uart4_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART4_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart4_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart4_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_uart5_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART5_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart5_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart5_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk blsp1_uart6_apps_clk_src = {
+	.cmd_rcgr_reg =  BLSP1_UART6_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_blsp1_uart1_6_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "blsp1_uart6_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 31580000, NOMINAL, 63160000),
+		CLK_INIT(blsp1_uart6_apps_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_ce1_clk[] = {
+	F(50000000, gpll0, 12, 0, 0),
+	F(100000000, gpll0, 6, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk ce1_clk_src = {
+	.cmd_rcgr_reg = CE1_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_gcc_ce1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "ce1_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 50000000, NOMINAL, 100000000),
+		CLK_INIT(ce1_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_gp1_3_clk[] = {
+	F(19200000, gcc_xo, 1, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk gp1_clk_src = {
+	.cmd_rcgr_reg =  GP1_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_gp1_3_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gp1_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(gp1_clk_src.c),
+	},
+};
+
+static struct rcg_clk gp2_clk_src = {
+	.cmd_rcgr_reg =  GP2_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_gp1_3_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gp2_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(gp2_clk_src.c),
+	},
+};
+
+static struct rcg_clk gp3_clk_src = {
+	.cmd_rcgr_reg =  GP3_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_gp1_3_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gp3_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(gp3_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_pdm2_clk[] = {
+	F(60000000, gpll0, 10, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk pdm2_clk_src = {
+	.cmd_rcgr_reg = PDM2_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_gcc_pdm2_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "pdm2_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP1(LOW, 120000000),
+		CLK_INIT(pdm2_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_sdcc1_2_apps_clk[] = {
+	F(   144000, gcc_xo, 16, 3, 25),
+	F(   400000, gcc_xo, 12, 1,  4),
+	F( 20000000,  gpll0, 15, 1,  2),
+	F( 25000000,  gpll0, 12, 1,  2),
+	F( 50000000,  gpll0, 12, 0,  0),
+	F(100000000,  gpll0,  6, 0,  0),
+	F(200000000,  gpll0,  3, 0,  0),
+	F_END,
+};
+
+static struct rcg_clk sdcc1_apps_clk_src = {
+	.cmd_rcgr_reg =  SDCC1_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_sdcc1_2_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "sdcc1_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(sdcc1_apps_clk_src.c),
+	},
+};
+
+static struct rcg_clk sdcc2_apps_clk_src = {
+	.cmd_rcgr_reg =  SDCC2_APPS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_gcc_sdcc1_2_apps_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "sdcc2_apps_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(sdcc2_apps_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_gcc_usb_hs_system_clk[] = {
+	F(75000000, gpll0, 8, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk usb_hs_system_clk_src = {
+	.cmd_rcgr_reg = USB_HS_SYSTEM_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_gcc_usb_hs_system_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "usb_hs_system_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 60000000, NOMINAL, 100000000),
+		CLK_INIT(usb_hs_system_clk_src.c),
+	},
+};
+
+static struct local_vote_clk gcc_blsp1_ahb_clk = {
+	.cbcr_reg = BLSP1_AHB_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(17),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_blsp1_ahb_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_blsp1_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup1_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP1_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup1_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup1_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup1_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP1_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup1_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup1_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup1_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup2_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP2_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup2_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup2_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup2_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP2_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup2_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup2_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup2_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup3_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP3_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup3_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup3_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup3_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP3_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup3_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup3_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup3_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup4_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP4_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup4_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup4_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup4_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP4_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup4_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup4_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup4_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup5_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP5_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup5_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup5_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup5_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP5_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup5_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup5_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup5_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup6_i2c_apps_clk = {
+	.cbcr_reg = BLSP1_QUP6_I2C_APPS_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup6_i2c_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup6_i2c_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_qup6_spi_apps_clk = {
+	.cbcr_reg = BLSP1_QUP6_SPI_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_qup6_spi_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_qup6_spi_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_qup6_spi_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart1_apps_clk = {
+	.cbcr_reg = BLSP1_UART1_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart1_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart1_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart1_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart2_apps_clk = {
+	.cbcr_reg = BLSP1_UART2_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart2_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart2_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart2_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart3_apps_clk = {
+	.cbcr_reg = BLSP1_UART3_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart3_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart3_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart3_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart4_apps_clk = {
+	.cbcr_reg = BLSP1_UART4_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart4_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart4_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart4_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart5_apps_clk = {
+	.cbcr_reg = BLSP1_UART5_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart5_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart5_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart5_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_blsp1_uart6_apps_clk = {
+	.cbcr_reg = BLSP1_UART6_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &blsp1_uart6_apps_clk_src.c,
+		.dbg_name = "gcc_blsp1_uart6_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_blsp1_uart6_apps_clk.c),
+	},
+};
+
+static struct local_vote_clk gcc_boot_rom_ahb_clk = {
+	.cbcr_reg = BOOT_ROM_AHB_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(10),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_boot_rom_ahb_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_boot_rom_ahb_clk.c),
+	},
+};
+
+static struct local_vote_clk gcc_ce1_ahb_clk = {
+	.cbcr_reg = CE1_AHB_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(3),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_ce1_ahb_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_ce1_ahb_clk.c),
+	},
+};
+
+static struct local_vote_clk gcc_ce1_axi_clk = {
+	.cbcr_reg = CE1_AXI_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(4),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_ce1_axi_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_ce1_axi_clk.c),
+	},
+};
+
+static struct local_vote_clk gcc_ce1_clk = {
+	.cbcr_reg = CE1_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(5),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_ce1_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_ce1_clk.c),
+	},
+};
+
+static struct branch_clk gcc_copss_smmu_ahb_clk = {
+	.cbcr_reg = COPSS_SMMU_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_copss_smmu_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_copss_smmu_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_lpss_smmu_ahb_clk = {
+	.cbcr_reg = LPSS_SMMU_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+			.dbg_name = "gcc_lpss_smmu_ahb_clk",
+			.ops = &clk_ops_branch,
+			CLK_INIT(gcc_lpss_smmu_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_gp1_clk = {
+	.cbcr_reg = GP1_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gp1_clk_src.c,
+		.dbg_name = "gcc_gp1_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_gp1_clk.c),
+	},
+};
+
+static struct branch_clk gcc_gp2_clk = {
+	.cbcr_reg = GP2_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gp2_clk_src.c,
+		.dbg_name = "gcc_gp2_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_gp2_clk.c),
+	},
+};
+
+static struct branch_clk gcc_gp3_clk = {
+	.cbcr_reg = GP3_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &gp3_clk_src.c,
+		.dbg_name = "gcc_gp3_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_gp3_clk.c),
+	},
+};
+
+static struct branch_clk gcc_lpass_q6_axi_clk = {
+	.cbcr_reg = LPASS_Q6_AXI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_lpass_q6_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_lpass_q6_axi_clk.c),
+	},
+};
+
+static struct branch_clk gcc_mmss_noc_cfg_ahb_clk = {
+	.cbcr_reg = MMSS_NOC_CFG_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_mmss_noc_cfg_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_mmss_noc_cfg_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_mss_cfg_ahb_clk = {
+	.cbcr_reg = MSS_CFG_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_mss_cfg_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_mss_cfg_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_mss_q6_bimc_axi_clk = {
+	.cbcr_reg = MSS_Q6_BIMC_AXI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_mss_q6_bimc_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_mss_q6_bimc_axi_clk.c),
+	},
+};
+
+static struct branch_clk gcc_pdm2_clk = {
+	.cbcr_reg = PDM2_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &pdm2_clk_src.c,
+		.dbg_name = "gcc_pdm2_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_pdm2_clk.c),
+	},
+};
+
+static struct branch_clk gcc_pdm_ahb_clk = {
+	.cbcr_reg = PDM_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_pdm_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_pdm_ahb_clk.c),
+	},
+};
+
+static struct local_vote_clk gcc_prng_ahb_clk = {
+	.cbcr_reg = PRNG_AHB_CBCR,
+	.vote_reg = APCS_CLOCK_BRANCH_ENA_VOTE,
+	.en_mask = BIT(13),
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_prng_ahb_clk",
+		.ops = &clk_ops_vote,
+		CLK_INIT(gcc_prng_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_sdcc1_ahb_clk = {
+	.cbcr_reg = SDCC1_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_sdcc1_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_sdcc1_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_sdcc1_apps_clk = {
+	.cbcr_reg = SDCC1_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &sdcc1_apps_clk_src.c,
+		.dbg_name = "gcc_sdcc1_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_sdcc1_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_sdcc2_ahb_clk = {
+	.cbcr_reg = SDCC2_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_sdcc2_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_sdcc2_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_sdcc2_apps_clk = {
+	.cbcr_reg = SDCC2_APPS_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &sdcc2_apps_clk_src.c,
+		.dbg_name = "gcc_sdcc2_apps_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_sdcc2_apps_clk.c),
+	},
+};
+
+static struct branch_clk gcc_usb2a_phy_sleep_clk = {
+	.cbcr_reg = USB2A_PHY_SLEEP_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_usb2a_phy_sleep_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_usb2a_phy_sleep_clk.c),
+	},
+};
+
+static struct branch_clk gcc_usb_hs_ahb_clk = {
+	.cbcr_reg = USB_HS_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.dbg_name = "gcc_usb_hs_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_usb_hs_ahb_clk.c),
+	},
+};
+
+static struct branch_clk gcc_usb_hs_system_clk = {
+	.cbcr_reg = USB_HS_SYSTEM_CBCR,
+	.has_sibling = 0,
+	.bcr_reg = USB_HS_BCR,
+	.base = &virt_bases[GCC_BASE],
+	.c = {
+		.parent = &usb_hs_system_clk_src.c,
+		.dbg_name = "gcc_usb_hs_system_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gcc_usb_hs_system_clk.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_csi0_1_clk[] = {
+	F_MM(100000000,  gpll0, 6, 0, 0),
+	F_MM(200000000, mmpll0, 4, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk csi0_clk_src = {
+	.cmd_rcgr_reg = CSI0_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_csi0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "csi0_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(csi0_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_mmss_mmssnoc_ahb_clk[] = {
+	F_MM(19200000, gcc_xo,  1, 0, 0),
+	F_MM(40000000,  gpll0, 15, 0, 0),
+	F_MM(80000000, mmpll0, 10, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk ahb_clk_src = {
+	.cmd_rcgr_reg = AHB_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_mmss_mmssnoc_ahb_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "ahb_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 40000000, NOMINAL, 80000000),
+		CLK_INIT(ahb_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_mmss_mmssnoc_axi_clk[] = {
+	F_MM( 19200000, gcc_xo,  1, 0, 0),
+	F_MM( 37500000,  gpll0, 16, 0, 0),
+	F_MM( 50000000,  gpll0, 12, 0, 0),
+	F_MM( 75000000,  gpll0,  8, 0, 0),
+	F_MM(100000000,  gpll0,  6, 0, 0),
+	F_MM(150000000,  gpll0,  4, 0, 0),
+	F_MM(200000000, mmpll0,  4, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk axi_clk_src = {
+	.cmd_rcgr_reg = AXI_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_mmss_mmssnoc_axi_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "axi_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(axi_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_dsi_pclk_clk[] = {
+	F_MDSS( 50000000, dsipll, 10, 0, 0),
+	F_MDSS(103330000, dsipll,  9, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk dsi_pclk_clk_src = {
+	.cmd_rcgr_reg =  DSI_PCLK_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_dsi_pclk_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "dsi_pclk_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 50000000, NOMINAL, 103330000),
+		CLK_INIT(dsi_pclk_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_oxili_gfx3d_clk[] = {
+	F_MM( 19200000, gcc_xo,  1, 0, 0),
+	F_MM( 37500000,  gpll0, 16, 0, 0),
+	F_MM( 50000000,  gpll0, 12, 0, 0),
+	F_MM( 75000000,  gpll0,  8, 0, 0),
+	F_MM(100000000,  gpll0,  6, 0, 0),
+	F_MM(150000000,  gpll0,  4, 0, 0),
+	F_MM(200000000,  gpll0,  3, 0, 0),
+	F_MM(300000000,  gpll0,  2, 0, 0),
+	F_MM(400000000, mmpll1,  3, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk gfx3d_clk_src = {
+	.cmd_rcgr_reg = GFX3D_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_oxili_gfx3d_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "gfx3d_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP3(LOW, 150000000, NOMINAL, 300000000, HIGH,
+					400000000),
+		CLK_INIT(gfx3d_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_vfe_clk[] = {
+	F_MM( 37500000,  gpll0,  16, 0, 0),
+	F_MM( 50000000,  gpll0,  12, 0, 0),
+	F_MM( 60000000,  gpll0,  10, 0, 0),
+	F_MM( 80000000,  gpll0, 7.5, 0, 0),
+	F_MM(100000000,  gpll0,   6, 0, 0),
+	F_MM(109090000,  gpll0, 5.5, 0, 0),
+	F_MM(133330000,  gpll0, 4.5, 0, 0),
+	F_MM(200000000,  gpll0,   3, 0, 0),
+	F_MM(228570000, mmpll0, 3.5, 0, 0),
+	F_MM(266670000, mmpll0,   3, 0, 0),
+	F_MM(320000000, mmpll0, 2.5, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk vfe_clk_src = {
+	.cmd_rcgr_reg = VFE_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_vfe_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "vfe_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP3(LOW, 133330000, NOMINAL, 266670000, HIGH,
+					320000000),
+		CLK_INIT(vfe_clk_src.c),
+	},
+};
+
+static struct rcg_clk csi1_clk_src = {
+	.cmd_rcgr_reg = CSI1_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_csi0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "csi1_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(csi1_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_csi0_1phytimer_clk[] = {
+	F_MM(100000000,  gpll0, 6, 0, 0),
+	F_MM(200000000, mmpll0, 4, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk csi0phytimer_clk_src = {
+	.cmd_rcgr_reg = CSI0PHYTIMER_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_csi0_1phytimer_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "csi0phytimer_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(csi0phytimer_clk_src.c),
+	},
+};
+
+static struct rcg_clk csi1phytimer_clk_src = {
+	.cmd_rcgr_reg = CSI1PHYTIMER_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_csi0_1phytimer_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "csi1phytimer_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 100000000, NOMINAL, 200000000),
+		CLK_INIT(csi1phytimer_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_dsi_clk[] = {
+	F_MDSS(155000000,  dsipll, 6, 0, 0),
+	F_MDSS(310000000,  dsipll, 3, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk dsi_clk_src = {
+	.cmd_rcgr_reg =  DSI_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_dsi_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "dsi_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 155000000, NOMINAL, 310000000),
+		CLK_INIT(dsi_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_dsi_byte_clk[] = {
+	F_MDSS( 62500000, dsipll, 12, 0, 0),
+	F_MDSS(125000000, dsipll,  6, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk dsi_byte_clk_src = {
+	.cmd_rcgr_reg = DSI_BYTE_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_dsi_byte_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "dsi_byte_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP2(LOW, 62500000, NOMINAL, 125000000),
+		CLK_INIT(dsi_byte_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_dsi_esc_clk[] = {
+	F_MM(19200000, gcc_xo, 1, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk dsi_esc_clk_src = {
+	.cmd_rcgr_reg = DSI_ESC_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_dsi_esc_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "dsi_esc_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP1(LOW, 19200000),
+		CLK_INIT(dsi_esc_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_mclk0_1_clk[] = {
+	F_MM(66670000, gpll0, 9, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk mclk0_clk_src = {
+	.cmd_rcgr_reg =  MCLK0_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_mclk0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mclk0_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP1(LOW, 66670000),
+		CLK_INIT(mclk0_clk_src.c),
+	},
+};
+
+static struct rcg_clk mclk1_clk_src = {
+	.cmd_rcgr_reg =  MCLK1_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_mclk0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mclk1_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP1(LOW, 66670000),
+		CLK_INIT(mclk1_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_mdp_vsync_clk[] = {
+	F_MM(19200000, gcc_xo, 1, 0, 0),
+	F_END,
+};
+
+static struct rcg_clk mdp_vsync_clk_src = {
+	.cmd_rcgr_reg = MDP_VSYNC_CMD_RCGR,
+	.set_rate = set_rate_hid,
+	.freq_tbl = ftbl_mdp_vsync_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mdp_vsync_clk_src",
+		.ops = &clk_ops_rcg,
+		VDD_DIG_FMAX_MAP1(LOW, 19200000),
+		CLK_INIT(mdp_vsync_clk_src.c),
+	},
+};
+
+static struct branch_clk bimc_gfx_clk = {
+	.cbcr_reg = BIMC_GFX_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "bimc_gfx_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(bimc_gfx_clk.c),
+	},
+};
+
+static struct branch_clk csi0_clk = {
+	.cbcr_reg = CSI0_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi0_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi0_clk.c),
+	},
+};
+
+static struct branch_clk csi0phy_clk = {
+	.cbcr_reg = CSI0PHY_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi0phy_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi0phy_clk.c),
+	},
+};
+
+static struct branch_clk csi0phytimer_clk = {
+	.cbcr_reg = CSI0PHYTIMER_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0phytimer_clk_src.c,
+		.dbg_name = "csi0phytimer_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi0phytimer_clk.c),
+	},
+};
+
+static struct branch_clk csi0pix_clk = {
+	.cbcr_reg = CSI0PIX_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi0pix_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi0pix_clk.c),
+	},
+};
+
+static struct branch_clk csi0rdi_clk = {
+	.cbcr_reg = CSI0RDI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi0rdi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi0rdi_clk.c),
+	},
+};
+
+static struct branch_clk csi1_clk = {
+	.cbcr_reg = CSI1_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi1_clk_src.c,
+		.dbg_name = "csi1_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi1_clk.c),
+	},
+};
+
+static struct branch_clk csi1phy_clk = {
+	.cbcr_reg = CSI1PHY_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi1_clk_src.c,
+		.dbg_name = "csi1phy_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi1phy_clk.c),
+	},
+};
+
+static struct branch_clk csi1phytimer_clk = {
+	.cbcr_reg = CSI1PHYTIMER_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi1phytimer_clk_src.c,
+		.dbg_name = "csi1phytimer_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi1phytimer_clk.c),
+	},
+};
+
+static struct branch_clk csi1pix_clk = {
+	.cbcr_reg = CSI1PIX_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi1pix_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi1pix_clk.c),
+	},
+};
+
+static struct branch_clk csi1rdi_clk = {
+	.cbcr_reg = CSI1RDI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &csi0_clk_src.c,
+		.dbg_name = "csi1rdi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi1rdi_clk.c),
+	},
+};
+
+static struct branch_clk csi_ahb_clk = {
+	.cbcr_reg = CSI_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "csi_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi_ahb_clk.c),
+	},
+};
+
+static struct branch_clk csi_vfe_clk = {
+	.cbcr_reg = CSI_VFE_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &vfe_clk_src.c,
+		.dbg_name = "csi_vfe_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(csi_vfe_clk.c),
+	},
+};
+
+static struct branch_clk dsi_clk = {
+	.cbcr_reg = DSI_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_clk_src.c,
+		.dbg_name = "dsi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(dsi_clk.c),
+	},
+};
+
+static struct branch_clk dsi_ahb_clk = {
+	.cbcr_reg = DSI_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "dsi_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(dsi_ahb_clk.c),
+	},
+};
+
+static struct branch_clk dsi_byte_clk = {
+	.cbcr_reg = DSI_BYTE_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_byte_clk_src.c,
+		.dbg_name = "dsi_byte_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(dsi_byte_clk.c),
+	},
+};
+
+static struct branch_clk dsi_esc_clk = {
+	.cbcr_reg = DSI_ESC_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_esc_clk_src.c,
+		.dbg_name = "dsi_esc_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(dsi_esc_clk.c),
+	},
+};
+
+static struct branch_clk dsi_pclk_clk = {
+	.cbcr_reg = DSI_PCLK_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_pclk_clk_src.c,
+		.dbg_name = "dsi_pclk_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(dsi_pclk_clk.c),
+	},
+};
+
+static struct branch_clk gmem_gfx3d_clk = {
+	.cbcr_reg = GMEM_GFX3D_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &gfx3d_clk_src.c,
+		.dbg_name = "gmem_gfx3d_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(gmem_gfx3d_clk.c),
+	},
+};
+
+static struct branch_clk mclk0_clk = {
+	.cbcr_reg = MCLK0_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &mclk0_clk_src.c,
+		.dbg_name = "mclk0_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mclk0_clk.c),
+	},
+};
+
+static struct branch_clk mclk1_clk = {
+	.cbcr_reg = MCLK1_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &mclk1_clk_src.c,
+		.dbg_name = "mclk1_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mclk1_clk.c),
+	},
+};
+
+static struct branch_clk mdp_ahb_clk = {
+	.cbcr_reg = MDP_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mdp_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mdp_ahb_clk.c),
+	},
+};
+
+static struct branch_clk mdp_axi_clk = {
+	.cbcr_reg = MDP_AXI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &axi_clk_src.c,
+		.dbg_name = "mdp_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mdp_axi_clk.c),
+	},
+};
+
+static struct branch_clk mdp_dsi_clk = {
+	.cbcr_reg = MDP_DSI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_pclk_clk_src.c,
+		.dbg_name = "mdp_dsi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mdp_dsi_clk.c),
+	},
+};
+
+static struct branch_clk mdp_lcdc_clk = {
+	.cbcr_reg = MDP_LCDC_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &dsi_pclk_clk_src.c,
+		.dbg_name = "mdp_lcdc_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mdp_lcdc_clk.c),
+	},
+};
+
+static struct branch_clk mdp_vsync_clk = {
+	.cbcr_reg = MDP_VSYNC_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &mdp_vsync_clk_src.c,
+		.dbg_name = "mdp_vsync_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mdp_vsync_clk.c),
+	},
+};
+
+static struct branch_clk mmss_misc_ahb_clk = {
+	.cbcr_reg = MMSS_MISC_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mmss_misc_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mmss_misc_ahb_clk.c),
+	},
+};
+
+static struct branch_clk mmss_mmssnoc_axi_clk = {
+	.cbcr_reg = MMSS_MMSSNOC_AXI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &axi_clk_src.c,
+		.dbg_name = "mmss_mmssnoc_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mmss_mmssnoc_axi_clk.c),
+	},
+};
+
+static struct branch_clk mmss_s0_axi_clk = {
+	.cbcr_reg = MMSS_S0_AXI_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &axi_clk_src.c,
+		.dbg_name = "mmss_s0_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mmss_s0_axi_clk.c),
+		.depends = &mmss_mmssnoc_axi_clk.c,
+	},
+};
+
+static struct branch_clk mmss_mmssnoc_ahb_clk = {
+	.cbcr_reg = MMSS_MMSSNOC_AHB_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &ahb_clk_src.c,
+		.dbg_name = "mmss_mmssnoc_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mmss_mmssnoc_ahb_clk.c),
+	},
+};
+
+static struct branch_clk mmss_mmssnoc_bto_ahb_clk = {
+	.cbcr_reg = MMSS_MMSSNOC_BTO_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "mmss_mmssnoc_bto_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(mmss_mmssnoc_bto_ahb_clk.c),
+	},
+};
+
+static struct branch_clk oxili_ahb_clk = {
+	.cbcr_reg = OXILI_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "oxili_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(oxili_ahb_clk.c),
+	},
+};
+
+static struct branch_clk oxili_gfx3d_clk = {
+	.cbcr_reg = OXILI_GFX3D_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &gfx3d_clk_src.c,
+		.dbg_name = "oxili_gfx3d_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(oxili_gfx3d_clk.c),
+	},
+};
+
+static struct branch_clk vfe_clk = {
+	.cbcr_reg = VFE_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &vfe_clk_src.c,
+		.dbg_name = "vfe_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(vfe_clk.c),
+	},
+};
+
+static struct branch_clk vfe_ahb_clk = {
+	.cbcr_reg = VFE_AHB_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.dbg_name = "vfe_ahb_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(vfe_ahb_clk.c),
+	},
+};
+
+static struct branch_clk vfe_axi_clk = {
+	.cbcr_reg = VFE_AXI_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[MMSS_BASE],
+	.c = {
+		.parent = &axi_clk_src.c,
+		.dbg_name = "vfe_axi_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(vfe_axi_clk.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_audio_core_lpaif_clk[] = {
+	F_LPASS(  512000, lpapll0, 16, 1, 60),
+	F_LPASS(  768000, lpapll0, 16, 1, 40),
+	F_LPASS( 1024000, lpapll0, 16, 1, 30),
+	F_LPASS( 1536000, lpapll0, 16, 1, 20),
+	F_LPASS( 2048000, lpapll0, 16, 1, 15),
+	F_LPASS( 3072000, lpapll0, 16, 1, 10),
+	F_LPASS( 4096000, lpapll0, 15, 1,  8),
+	F_LPASS( 6144000, lpapll0, 10, 1,  8),
+	F_LPASS( 8192000, lpapll0, 15, 1,  4),
+	F_LPASS(12288000, lpapll0, 10, 1,  4),
+	F_END,
+};
+
+static struct rcg_clk lpaif_pri_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_PRI_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_pri_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12290000, NOMINAL, 24580000),
+		CLK_INIT(lpaif_pri_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_quad_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_QUAD_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_quad_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12290000, NOMINAL, 24580000),
+		CLK_INIT(lpaif_quad_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_sec_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_SEC_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_sec_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12290000, NOMINAL, 24580000),
+		CLK_INIT(lpaif_sec_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_spkr_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_SPKR_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_spkr_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12290000, NOMINAL, 24580000),
+		CLK_INIT(lpaif_spkr_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_ter_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_TER_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_ter_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12290000, NOMINAL, 24580000),
+		CLK_INIT(lpaif_ter_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_audio_core_lpaif_pcm0_1_clk[] = {
+	F_LPASS( 512000, lpapll0, 16, 1, 60),
+	F_LPASS( 768000, lpapll0, 16, 1, 40),
+	F_LPASS(1024000, lpapll0, 16, 1, 30),
+	F_LPASS(1536000, lpapll0, 16, 1, 20),
+	F_LPASS(2048000, lpapll0, 16, 1, 15),
+	F_LPASS(3072000, lpapll0, 16, 1, 10),
+	F_LPASS(4096000, lpapll0, 15, 1,  8),
+	F_LPASS(6144000, lpapll0, 10, 1,  8),
+	F_LPASS(8192000, lpapll0, 15, 1,  4),
+	F_END,
+};
+
+static struct rcg_clk lpaif_pcm0_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_PCM0_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_pcm0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_pcm0_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 4100000, NOMINAL, 8192000),
+		CLK_INIT(lpaif_pcm0_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_pcm1_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_PCM1_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_pcm0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_pcm1_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 4100000, NOMINAL, 8192000),
+		CLK_INIT(lpaif_pcm1_clk_src.c),
+	},
+};
+
+static struct rcg_clk lpaif_pcmoe_clk_src = {
+	.cmd_rcgr_reg =  LPAIF_PCMOE_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_lpaif_pcm0_1_clk,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "lpaif_pcmoe_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 6140000, NOMINAL, 12290000),
+		CLK_INIT(lpaif_pcmoe_clk_src.c),
+	},
+};
+
+static struct clk_freq_tbl ftbl_audio_core_slimbus_core_clock[] = {
+	F_LPASS(24576000, lpapll0, 4, 1, 5),
+	F_END
+};
+
+static struct rcg_clk audio_core_slimbus_core_clk_src = {
+	.cmd_rcgr_reg = SLIMBUS_CMD_RCGR,
+	.set_rate = set_rate_mnd,
+	.freq_tbl = ftbl_audio_core_slimbus_core_clock,
+	.current_freq = &rcg_dummy_freq,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_slimbus_core_clk_src",
+		.ops = &clk_ops_rcg_mnd,
+		VDD_DIG_FMAX_MAP2(LOW, 12935000, NOMINAL, 25869000),
+		CLK_INIT(audio_core_slimbus_core_clk_src.c),
+	},
+};
+
+static struct branch_clk audio_core_slimbus_core_clk = {
+	.cbcr_reg = AUDIO_CORE_SLIMBUS_CORE_CBCR,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &audio_core_slimbus_core_clk_src.c,
+		.dbg_name = "audio_core_slimbus_core_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_slimbus_core_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_ixfabric_clk = {
+	.cbcr_reg = AUDIO_CORE_IXFABRIC_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_ixfabric_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_ixfabric_clk.c),
+	},
+};
+
+static struct branch_clk audio_wrapper_br_clk = {
+	.cbcr_reg = AUDIO_WRAPPER_BR_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_wrapper_br_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_wrapper_br_clk.c),
+	},
+};
+
+static struct branch_clk q6ss_ahb_lfabif_clk = {
+	.cbcr_reg = Q6SS_AHB_LFABIF_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "q6ss_ahb_lfabif_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(q6ss_ahb_lfabif_clk.c),
+	},
+};
+
+static struct branch_clk q6ss_ahbm_clk = {
+	.cbcr_reg = Q6SS_AHBM_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "q6ss_ahbm_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(q6ss_ahbm_clk.c),
+	},
+};
+
+static struct branch_clk q6ss_xo_clk = {
+	.cbcr_reg = Q6SS_XO_CBCR,
+	.has_sibling = 1,
+	.bcr_reg = LPASS_Q6SS_BCR,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &gcc_xo_clk_src.c,
+		.dbg_name = "q6ss_xo_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(q6ss_xo_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pcm_data_oe_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PCM_DATA_OE_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_pcmoe_clk_src.c,
+		.dbg_name = "audio_core_lpaif_pcm_data_oe_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pcm_data_oe_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pri_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PRI_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_pri_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pri_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pri_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PRI_IBIT_CBCR,
+	.has_sibling = 0,
+	.max_div = 511,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_pri_clk_src.c,
+		.dbg_name = "audio_core_lpaif_pri_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pri_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pri_osr_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PRI_OSR_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_pri_clk_src.c,
+		.dbg_name = "audio_core_lpaif_pri_osr_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pri_osr_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pcm0_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PCM0_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_pcm0_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pcm0_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pcm0_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PCM0_IBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_pcm0_clk_src.c,
+		.dbg_name = "audio_core_lpaif_pcm0_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pcm0_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_quad_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_QUAD_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_quad_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_quad_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_quad_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_QUAD_IBIT_CBCR,
+	.has_sibling = 0,
+	.max_div = 511,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_quad_clk_src.c,
+		.dbg_name = "audio_core_lpaif_quad_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_quad_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_quad_osr_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_QUAD_OSR_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_quad_clk_src.c,
+		.dbg_name = "audio_core_lpaif_quad_osr_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_quad_osr_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_sec_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_SEC_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_sec_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_sec_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_sec_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_SEC_IBIT_CBCR,
+	.has_sibling = 0,
+	.max_div = 511,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_sec_clk_src.c,
+		.dbg_name = "audio_core_lpaif_sec_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_sec_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_sec_osr_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_SEC_OSR_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_sec_clk_src.c,
+		.dbg_name = "audio_core_lpaif_sec_osr_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_sec_osr_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pcm1_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PCM1_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_pcm1_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pcm1_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_pcm1_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_PCM1_IBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_pcm1_clk_src.c,
+		.dbg_name = "audio_core_lpaif_pcm1_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_pcm1_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_codec_spkr_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_CODEC_SPKR_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_codec_spkr_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_codec_spkr_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_codec_spkr_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_CODEC_SPKR_IBIT_CBCR,
+	.has_sibling = 1,
+	.max_div = 511,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_spkr_clk_src.c,
+		.dbg_name = "audio_core_lpaif_codec_spkr_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_codec_spkr_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_codec_spkr_osr_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_CODEC_SPKR_OSR_CBCR,
+	.has_sibling = 1,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_spkr_clk_src.c,
+		.dbg_name = "audio_core_lpaif_codec_spkr_osr_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_codec_spkr_osr_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_ter_ebit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_TER_EBIT_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.dbg_name = "audio_core_lpaif_ter_ebit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_ter_ebit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_ter_ibit_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_TER_IBIT_CBCR,
+	.has_sibling = 0,
+	.max_div = 511,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_ter_clk_src.c,
+		.dbg_name = "audio_core_lpaif_ter_ibit_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_ter_ibit_clk.c),
+	},
+};
+
+static struct branch_clk audio_core_lpaif_ter_osr_clk = {
+	.cbcr_reg = AUDIO_CORE_LPAIF_TER_OSR_CBCR,
+	.has_sibling = 0,
+	.base = &virt_bases[LPASS_BASE],
+	.c = {
+		.parent = &lpaif_ter_clk_src.c,
+		.dbg_name = "audio_core_lpaif_ter_osr_clk",
+		.ops = &clk_ops_branch,
+		CLK_INIT(audio_core_lpaif_ter_osr_clk.c),
+	},
+};
+
+#ifdef CONFIG_DEBUG_FS
+
+struct measure_mux_entry {
+	struct clk *c;
+	int base;
+	u32 debug_mux;
+};
+
+static struct measure_mux_entry measure_mux[] = {
+	{                   &snoc_clk.c, GCC_BASE, 0x0000},
+	{                   &cnoc_clk.c, GCC_BASE, 0x0008},
+	{     &gcc_copss_smmu_ahb_clk.c, GCC_BASE, 0x000c},
+	{      &gcc_lpss_smmu_ahb_clk.c, GCC_BASE, 0x000d},
+	{                   &pnoc_clk.c, GCC_BASE, 0x0010},
+	{   &gcc_mmss_noc_cfg_ahb_clk.c, GCC_BASE, 0x002a},
+	{        &gcc_mss_cfg_ahb_clk.c, GCC_BASE, 0x0030},
+	{    &gcc_mss_q6_bimc_axi_clk.c, GCC_BASE, 0x0031},
+	{      &gcc_usb_hs_system_clk.c, GCC_BASE, 0x0060},
+	{         &gcc_usb_hs_ahb_clk.c, GCC_BASE, 0x0061},
+	{    &gcc_usb2a_phy_sleep_clk.c, GCC_BASE, 0x0063},
+	{         &gcc_sdcc1_apps_clk.c, GCC_BASE, 0x0068},
+	{          &gcc_sdcc1_ahb_clk.c, GCC_BASE, 0x0069},
+	{         &gcc_sdcc2_apps_clk.c, GCC_BASE, 0x0070},
+	{          &gcc_sdcc2_ahb_clk.c, GCC_BASE, 0x0071},
+	{          &gcc_blsp1_ahb_clk.c, GCC_BASE, 0x0088},
+	{&gcc_blsp1_qup1_spi_apps_clk.c, GCC_BASE, 0x008a},
+	{&gcc_blsp1_qup1_i2c_apps_clk.c, GCC_BASE, 0x008b},
+	{   &gcc_blsp1_uart1_apps_clk.c, GCC_BASE, 0x008c},
+	{&gcc_blsp1_qup2_spi_apps_clk.c, GCC_BASE, 0x008e},
+	{&gcc_blsp1_qup2_i2c_apps_clk.c, GCC_BASE, 0x0090},
+	{   &gcc_blsp1_uart2_apps_clk.c, GCC_BASE, 0x0091},
+	{&gcc_blsp1_qup3_spi_apps_clk.c, GCC_BASE, 0x0093},
+	{&gcc_blsp1_qup3_i2c_apps_clk.c, GCC_BASE, 0x0094},
+	{   &gcc_blsp1_uart3_apps_clk.c, GCC_BASE, 0x0095},
+	{&gcc_blsp1_qup4_spi_apps_clk.c, GCC_BASE, 0x0098},
+	{&gcc_blsp1_qup4_i2c_apps_clk.c, GCC_BASE, 0x0099},
+	{   &gcc_blsp1_uart4_apps_clk.c, GCC_BASE, 0x009a},
+	{&gcc_blsp1_qup5_spi_apps_clk.c, GCC_BASE, 0x009c},
+	{&gcc_blsp1_qup5_i2c_apps_clk.c, GCC_BASE, 0x009d},
+	{   &gcc_blsp1_uart5_apps_clk.c, GCC_BASE, 0x009e},
+	{&gcc_blsp1_qup6_spi_apps_clk.c, GCC_BASE, 0x00a1},
+	{&gcc_blsp1_qup6_i2c_apps_clk.c, GCC_BASE, 0x00a2},
+	{   &gcc_blsp1_uart6_apps_clk.c, GCC_BASE, 0x00a3},
+	{            &gcc_pdm_ahb_clk.c, GCC_BASE, 0x00d0},
+	{               &gcc_pdm2_clk.c, GCC_BASE, 0x00d2},
+	{           &gcc_prng_ahb_clk.c, GCC_BASE, 0x00d8},
+	{       &gcc_boot_rom_ahb_clk.c, GCC_BASE, 0x00f8},
+	{                &gcc_ce1_clk.c, GCC_BASE, 0x0138},
+	{            &gcc_ce1_axi_clk.c, GCC_BASE, 0x0139},
+	{            &gcc_ce1_ahb_clk.c, GCC_BASE, 0x013a},
+	{             &gcc_xo_clk_src.c, GCC_BASE, 0x0149},
+	{                   &bimc_clk.c, GCC_BASE, 0x0154},
+	{       &gcc_lpass_q6_axi_clk.c, GCC_BASE, 0x0160},
+
+	{&mmss_mmssnoc_ahb_clk.c, MMSS_BASE, 0x0001},
+	{   &mmss_misc_ahb_clk.c, MMSS_BASE, 0x0003},
+	{&mmss_mmssnoc_axi_clk.c, MMSS_BASE, 0x0004},
+	{     &mmss_s0_axi_clk.c, MMSS_BASE, 0x0005},
+	{       &oxili_ahb_clk.c, MMSS_BASE, 0x0007},
+	{     &oxili_gfx3d_clk.c, MMSS_BASE, 0x0008},
+	{      &gmem_gfx3d_clk.c, MMSS_BASE, 0x0009},
+	{         &mdp_axi_clk.c, MMSS_BASE, 0x000a},
+	{       &mdp_vsync_clk.c, MMSS_BASE, 0x000b},
+	{         &mdp_ahb_clk.c, MMSS_BASE, 0x000c},
+	{        &dsi_pclk_clk.c, MMSS_BASE, 0x000d},
+	{         &mdp_dsi_clk.c, MMSS_BASE, 0x000e},
+	{        &mdp_lcdc_clk.c, MMSS_BASE, 0x000f},
+	{             &dsi_clk.c, MMSS_BASE, 0x0010},
+	{        &dsi_byte_clk.c, MMSS_BASE, 0x0011},
+	{         &dsi_esc_clk.c, MMSS_BASE, 0x0012},
+	{         &dsi_ahb_clk.c, MMSS_BASE, 0x0013},
+	{           &mclk0_clk.c, MMSS_BASE, 0x0015},
+	{           &mclk1_clk.c, MMSS_BASE, 0x0016},
+	{    &csi0phytimer_clk.c, MMSS_BASE, 0x0017},
+	{    &csi1phytimer_clk.c, MMSS_BASE, 0x0018},
+	{             &vfe_clk.c, MMSS_BASE, 0x0019},
+	{         &vfe_ahb_clk.c, MMSS_BASE, 0x001a},
+	{         &vfe_axi_clk.c, MMSS_BASE, 0x001b},
+	{         &csi_vfe_clk.c, MMSS_BASE, 0x001c},
+	{            &csi0_clk.c, MMSS_BASE, 0x001d},
+	{         &csi_ahb_clk.c, MMSS_BASE, 0x001e},
+	{         &csi0phy_clk.c, MMSS_BASE, 0x001f},
+	{         &csi0rdi_clk.c, MMSS_BASE, 0x0020},
+	{         &csi0pix_clk.c, MMSS_BASE, 0x0021},
+	{            &csi1_clk.c, MMSS_BASE, 0x0022},
+	{         &csi1phy_clk.c, MMSS_BASE, 0x0023},
+	{         &csi1rdi_clk.c, MMSS_BASE, 0x0024},
+	{         &csi1pix_clk.c, MMSS_BASE, 0x0025},
+	{        &bimc_gfx_clk.c, MMSS_BASE, 0x0032},
+
+	{             &lpaif_pcmoe_clk_src.c, LPASS_BASE, 0x000f},
+	{              &lpaif_pcm1_clk_src.c, LPASS_BASE, 0x0012},
+	{              &lpaif_pcm0_clk_src.c, LPASS_BASE, 0x0013},
+	{              &lpaif_quad_clk_src.c, LPASS_BASE, 0x0014},
+	{               &lpaif_ter_clk_src.c, LPASS_BASE, 0x0015},
+	{               &lpaif_sec_clk_src.c, LPASS_BASE, 0x0016},
+	{               &lpaif_pri_clk_src.c, LPASS_BASE, 0x0017},
+	{              &lpaif_spkr_clk_src.c, LPASS_BASE, 0x0018},
+	{                   &q6ss_ahbm_clk.c, LPASS_BASE, 0x001d},
+	{             &q6ss_ahb_lfabif_clk.c, LPASS_BASE, 0x001e},
+	{            &audio_wrapper_br_clk.c, LPASS_BASE, 0x0022},
+	{                     &q6ss_xo_clk.c, LPASS_BASE, 0x002b},
+	{&audio_core_lpaif_pcm_data_oe_clk.c, LPASS_BASE, 0x0030},
+	{         &audio_core_ixfabric_clk.c, LPASS_BASE, 0x0059},
+
+	{&dummy_clk, N_BASES, 0x0000},
+};
+
+#define GCC_DEBUG_CLK_CTL		0x1880
+#define MMSS_DEBUG_CLK_CTL		0x0900
+#define LPASS_DEBUG_CLK_CTL		0x29000
+#define GLB_CLK_DIAG			0x001C
+
+static int measure_clk_set_parent(struct clk *c, struct clk *parent)
+{
+	struct measure_clk *clk = to_measure_clk(c);
+	unsigned long flags;
+	u32 regval, clk_sel, i;
+
+	if (!parent)
+		return -EINVAL;
+
+	for (i = 0; i < (ARRAY_SIZE(measure_mux) - 1); i++)
+		if (measure_mux[i].c == parent)
+			break;
+
+	if (measure_mux[i].c == &dummy_clk)
+		return -EINVAL;
+
+	spin_lock_irqsave(&local_clock_reg_lock, flags);
+	/*
+	 * Program the test vector, measurement period (sample_ticks)
+	 * and scaling multiplier.
+	 */
+	clk->sample_ticks = 0x10000;
+	clk->multiplier = 1;
+
+	switch (measure_mux[i].base) {
+
+	case GCC_BASE:
+		writel_relaxed(0, GCC_REG_BASE(GCC_DEBUG_CLK_CTL));
+		clk_sel = measure_mux[i].debug_mux;
+		break;
+
+	case MMSS_BASE:
+		writel_relaxed(0, MMSS_REG_BASE(MMSS_DEBUG_CLK_CTL));
+		clk_sel = 0x02C;
+		regval = BVAL(11, 0, measure_mux[i].debug_mux);
+		writel_relaxed(regval, MMSS_REG_BASE(MMSS_DEBUG_CLK_CTL));
+
+		/* Activate debug clock output */
+		regval |= BIT(16);
+		writel_relaxed(regval, MMSS_REG_BASE(MMSS_DEBUG_CLK_CTL));
+		break;
+
+	case LPASS_BASE:
+		writel_relaxed(0, LPASS_REG_BASE(LPASS_DEBUG_CLK_CTL));
+		clk_sel = 0x161;
+		regval = BVAL(11, 0, measure_mux[i].debug_mux);
+		writel_relaxed(regval, LPASS_REG_BASE(LPASS_DEBUG_CLK_CTL));
+
+		/* Activate debug clock output */
+		regval |= BIT(20);
+		writel_relaxed(regval, LPASS_REG_BASE(LPASS_DEBUG_CLK_CTL));
+		break;
+
+	case APCS_BASE:
+		clk->multiplier = 4;
+		clk_sel = 0x16A;
+		regval = measure_mux[i].debug_mux;
+		writel_relaxed(regval, APCS_REG_BASE(GLB_CLK_DIAG));
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	/* Set debug mux clock index */
+	regval = BVAL(8, 0, clk_sel);
+	writel_relaxed(regval, GCC_REG_BASE(GCC_DEBUG_CLK_CTL));
+
+	/* Activate debug clock output */
+	regval |= BIT(16);
+	writel_relaxed(regval, GCC_REG_BASE(GCC_DEBUG_CLK_CTL));
+
+	/* Make sure test vector is set before starting measurements. */
+	mb();
+	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
+
+	return 0;
+}
+
+#define CLOCK_FRQ_MEASURE_CTL		0x1884
+#define CLOCK_FRQ_MEASURE_STATUS	0x1888
+
+/* Sample clock for 'ticks' reference clock ticks. */
+static u32 run_measurement(unsigned ticks)
+{
+	/* Stop counters and set the XO4 counter start value. */
+	writel_relaxed(ticks, GCC_REG_BASE(CLOCK_FRQ_MEASURE_CTL));
+
+	/* Wait for timer to become ready. */
+	while ((readl_relaxed(GCC_REG_BASE(CLOCK_FRQ_MEASURE_STATUS)) &
+			BIT(25)) != 0)
+		cpu_relax();
+
+	/* Run measurement and wait for completion. */
+	writel_relaxed(BIT(20)|ticks, GCC_REG_BASE(CLOCK_FRQ_MEASURE_CTL));
+	while ((readl_relaxed(GCC_REG_BASE(CLOCK_FRQ_MEASURE_STATUS)) &
+			BIT(25)) == 0)
+		cpu_relax();
+
+	/* Return measured ticks. */
+	return readl_relaxed(GCC_REG_BASE(CLOCK_FRQ_MEASURE_STATUS)) &
+				BM(24, 0);
+}
+
+#define GCC_XO_DIV4_CBCR	0x10C8
+#define PLLTEST_PAD_CFG		0x188C
+
+/*
+ * Perform a hardware rate measurement for a given clock.
+ * FOR DEBUG USE ONLY: Measurements take ~15 ms!
+ */
+static unsigned long measure_clk_get_rate(struct clk *c)
+{
+	unsigned long flags;
+	u32 gcc_xo4_reg_backup;
+	u64 raw_count_short, raw_count_full;
+	struct measure_clk *clk = to_measure_clk(c);
+	unsigned ret;
+
+	ret = clk_prepare_enable(&gcc_xo_clk_src.c);
+	if (ret) {
+		pr_warning("CXO clock failed to enable. Can't measure\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&local_clock_reg_lock, flags);
+
+	/* Enable CXO/4 and RINGOSC branch. */
+	gcc_xo4_reg_backup = readl_relaxed(GCC_REG_BASE(GCC_XO_DIV4_CBCR));
+	writel_relaxed(0x1, GCC_REG_BASE(GCC_XO_DIV4_CBCR));
+
+	/*
+	 * The ring oscillator counter will not reset if the measured clock
+	 * is not running.  To detect this, run a short measurement before
+	 * the full measurement.  If the raw results of the two are the same
+	 * then the clock must be off.
+	 */
+
+	/* Run a short measurement. (~1 ms) */
+	raw_count_short = run_measurement(0x1000);
+	/* Run a full measurement. (~14 ms) */
+	raw_count_full = run_measurement(clk->sample_ticks);
+
+	writel_relaxed(gcc_xo4_reg_backup, GCC_REG_BASE(GCC_XO_DIV4_CBCR));
+
+	/* Return 0 if the clock is off. */
+	if (raw_count_full == raw_count_short) {
+		ret = 0;
+	} else {
+		/* Compute rate in Hz. */
+		raw_count_full = ((raw_count_full * 10) + 15) * 4800000;
+		do_div(raw_count_full, ((clk->sample_ticks * 10) + 35));
+		ret = (raw_count_full * clk->multiplier);
+	}
+
+	writel_relaxed(0x51A00, GCC_REG_BASE(PLLTEST_PAD_CFG));
+	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
+
+	clk_disable_unprepare(&gcc_xo_clk_src.c);
+
+	return ret;
+}
+#else /* !CONFIG_DEBUG_FS */
+static int measure_clk_set_parent(struct clk *clk, struct clk *parent)
+{
+	return -EINVAL;
+}
+
+static unsigned long measure_clk_get_rate(struct clk *clk)
+{
+	return 0;
+}
+#endif /* CONFIG_DEBUG_FS */
+
+static struct clk_ops clk_ops_measure = {
+	.set_parent = measure_clk_set_parent,
+	.get_rate = measure_clk_get_rate,
+};
+
+static struct measure_clk measure_clk = {
+	.c = {
+		.dbg_name = "measure_clk",
+		.ops = &clk_ops_measure,
+		CLK_INIT(measure_clk.c),
+	},
+	.multiplier = 1,
+};
+
+static struct clk_lookup msm_clocks_8910[] = {
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "msm_otg"),
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "fe200000.qcom,lpass"),
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "pil-q6v5-mss"),
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "pil-mba"),
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "fb000000.qcom,wcnss-wlan"),
+	CLK_LOOKUP("xo",	gcc_xo_clk_src.c, "pil_pronto"),
+	CLK_LOOKUP("measure",	measure_clk.c,	"debug"),
+
+	CLK_LOOKUP("iface_clk",  gcc_blsp1_ahb_clk.c, "f991f000.serial"),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_uart3_apps_clk.c, "f991f000.serial"),
+
+	CLK_LOOKUP("dfab_clk", pnoc_sps_clk.c, "msm_sps"),
+	CLK_LOOKUP("bus_clk",  pnoc_qseecom_clk.c, "qseecom"),
+
+	CLK_LOOKUP("bus_clk", snoc_clk.c, ""),
+	CLK_LOOKUP("bus_clk", pnoc_clk.c, ""),
+	CLK_LOOKUP("bus_clk", cnoc_clk.c, ""),
+	CLK_LOOKUP("mem_clk", bimc_clk.c, ""),
+	CLK_LOOKUP("bus_clk", snoc_a_clk.c, ""),
+	CLK_LOOKUP("bus_clk", pnoc_a_clk.c, ""),
+	CLK_LOOKUP("bus_clk", cnoc_a_clk.c, ""),
+	CLK_LOOKUP("mem_clk", bimc_a_clk.c, ""),
+
+	CLK_LOOKUP("bus_clk",	cnoc_msmbus_clk.c,	"msm_config_noc"),
+	CLK_LOOKUP("bus_a_clk",	cnoc_msmbus_a_clk.c,	"msm_config_noc"),
+	CLK_LOOKUP("bus_clk",	snoc_msmbus_clk.c,	"msm_sys_noc"),
+	CLK_LOOKUP("bus_a_clk",	snoc_msmbus_a_clk.c,	"msm_sys_noc"),
+	CLK_LOOKUP("bus_clk",	pnoc_msmbus_clk.c,	"msm_periph_noc"),
+	CLK_LOOKUP("bus_a_clk",	pnoc_msmbus_a_clk.c,	"msm_periph_noc"),
+	CLK_LOOKUP("mem_clk",	bimc_msmbus_clk.c,	"msm_bimc"),
+	CLK_LOOKUP("mem_a_clk",	bimc_msmbus_a_clk.c,	"msm_bimc"),
+	CLK_LOOKUP("mem_clk",	bimc_acpu_a_clk.c,	""),
+
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-tmc-etr"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-tpiu"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-replicator"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-tmc-etf"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-funnel-merg"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-funnel-in0"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-funnel-in1"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-funnel-kpss"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-funnel-mmss"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-stm"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-etm0"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-etm1"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-etm2"),
+	CLK_LOOKUP("core_clk", qdss_clk.c, "coresight-etm3"),
+
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-tmc-etr"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-tpiu"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-replicator"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-tmc-etf"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-funnel-merg"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-funnel-in0"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-funnel-in1"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-funnel-kpss"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-funnel-mmss"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-stm"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-etm0"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-etm1"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-etm2"),
+	CLK_LOOKUP("core_a_clk", qdss_a_clk.c, "coresight-etm3"),
+
+	CLK_LOOKUP("core_clk_src", blsp1_qup1_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src", blsp1_qup2_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src", blsp1_qup3_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src", blsp1_qup4_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src", blsp1_qup5_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src", blsp1_qup6_spi_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart1_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart2_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart3_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart4_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart5_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",    blsp1_uart6_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 ce1_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 gp1_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 gp2_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 gp3_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                pdm2_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",          sdcc1_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",          sdcc2_apps_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",       usb_hs_system_clk_src.c, ""),
+
+	CLK_LOOKUP("iface_clk",            gcc_blsp1_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup1_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup1_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup2_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup2_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup3_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup3_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup4_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup4_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup5_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup5_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup6_i2c_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",  gcc_blsp1_qup6_spi_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart1_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart2_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart3_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart4_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart5_apps_clk.c, ""),
+	CLK_LOOKUP("core_clk",     gcc_blsp1_uart6_apps_clk.c, ""),
+	CLK_LOOKUP("iface_clk",         gcc_boot_rom_ahb_clk.c, ""),
+	CLK_LOOKUP("iface_clk",              gcc_ce1_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",              gcc_ce1_axi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  gcc_ce1_clk.c, ""),
+	CLK_LOOKUP("iface_clk",       gcc_copss_smmu_ahb_clk.c, ""),
+	CLK_LOOKUP("iface_clk",        gcc_lpss_smmu_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  gcc_gp1_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  gcc_gp2_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  gcc_gp3_clk.c, ""),
+	CLK_LOOKUP("core_clk",         gcc_lpass_q6_axi_clk.c, ""),
+	CLK_LOOKUP("iface_clk",          gcc_mss_cfg_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",      gcc_mss_q6_bimc_axi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                 gcc_pdm2_clk.c, ""),
+	CLK_LOOKUP("iface_clk",              gcc_pdm_ahb_clk.c, ""),
+	CLK_LOOKUP("iface_clk",             gcc_prng_ahb_clk.c, ""),
+	CLK_LOOKUP("iface_clk",            gcc_sdcc1_ahb_clk.c, "msm_sdcc.1"),
+	CLK_LOOKUP("core_clk",           gcc_sdcc1_apps_clk.c, "msm_sdcc.1"),
+	CLK_LOOKUP("iface_clk",            gcc_sdcc2_ahb_clk.c, "msm_sdcc.2"),
+	CLK_LOOKUP("core_clk",           gcc_sdcc2_apps_clk.c, "msm_sdcc.2"),
+	CLK_LOOKUP("core_clk",      gcc_usb2a_phy_sleep_clk.c, ""),
+	CLK_LOOKUP("iface_clk",           gcc_usb_hs_ahb_clk.c, "f9a55000.usb"),
+	CLK_LOOKUP("core_clk",        gcc_usb_hs_system_clk.c, "f9a55000.usb"),
+
+	CLK_LOOKUP("core_clk_src",                csi0_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 axi_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",            dsi_pclk_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",               gfx3d_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 vfe_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                csi1_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",        csi0phytimer_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",        csi1phytimer_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 dsi_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",            dsi_byte_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",             dsi_esc_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",               mclk0_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",               mclk1_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",           mdp_vsync_clk_src.c, ""),
+
+	CLK_LOOKUP("core_clk",                 bimc_gfx_clk.c, ""),
+	CLK_LOOKUP("core_clk",                     csi0_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi0phy_clk.c, ""),
+	CLK_LOOKUP("core_clk",             csi0phytimer_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi0pix_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi0rdi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                     csi1_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi1phy_clk.c, ""),
+	CLK_LOOKUP("core_clk",             csi1phytimer_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi1pix_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi1rdi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  csi_vfe_clk.c, ""),
+	CLK_LOOKUP("core_clk",                      dsi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  dsi_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",                 dsi_byte_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  dsi_esc_clk.c, ""),
+	CLK_LOOKUP("core_clk",                 dsi_pclk_clk.c, ""),
+	CLK_LOOKUP("core_clk",               gmem_gfx3d_clk.c, ""),
+	CLK_LOOKUP("core_clk",                    mclk0_clk.c, ""),
+	CLK_LOOKUP("core_clk",                    mclk1_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  mdp_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  mdp_axi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  mdp_dsi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                 mdp_lcdc_clk.c, ""),
+	CLK_LOOKUP("core_clk",                mdp_vsync_clk.c, ""),
+	CLK_LOOKUP("core_clk",            mmss_misc_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",              mmss_s0_axi_clk.c, ""),
+	CLK_LOOKUP("core_clk",         mmss_mmssnoc_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",     mmss_mmssnoc_bto_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",         mmss_mmssnoc_axi_clk.c, ""),
+	CLK_LOOKUP("core_clk",                      vfe_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  vfe_ahb_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  vfe_axi_clk.c, ""),
+
+	CLK_LOOKUP("core_clk",   oxili_gfx3d_clk.c, "fdc00000.qcom,kgsl-3d0"),
+	CLK_LOOKUP("iface_clk",    oxili_ahb_clk.c, "fdc00000.qcom,kgsl-3d0"),
+	CLK_LOOKUP("mem_iface_clk", bimc_gfx_clk.c, "fdc00000.qcom,kgsl-3d0"),
+	CLK_LOOKUP("mem_clk",     gmem_gfx3d_clk.c, "fdc00000.qcom,kgsl-3d0"),
+
+	CLK_LOOKUP("iface_clk",           vfe_ahb_clk.c, "fd890000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",            vfe_axi_clk.c, "fd890000.qcom,iommu"),
+	CLK_LOOKUP("iface_clk",           mdp_ahb_clk.c, "fd860000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",            mdp_axi_clk.c, "fd860000.qcom,iommu"),
+	CLK_LOOKUP("iface_clk",           mdp_ahb_clk.c, "fd870000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",            mdp_axi_clk.c, "fd870000.qcom,iommu"),
+	CLK_LOOKUP("iface_clk",         oxili_ahb_clk.c, "fd880000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",           bimc_gfx_clk.c, "fd880000.qcom,iommu"),
+	CLK_LOOKUP("iface_clk", gcc_lpss_smmu_ahb_clk.c, "fd000000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",   gcc_lpass_q6_axi_clk.c, "fd000000.qcom,iommu"),
+	CLK_LOOKUP("iface_clk", gcc_copss_smmu_ahb_clk.c,
+							 "fd010000.qcom,iommu"),
+	CLK_LOOKUP("core_clk",         pnoc_iommu_clk.c, "fd010000.qcom,iommu"),
+
+	CLK_LOOKUP("core_clk_src",                 lpaif_pri_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                lpaif_quad_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 lpaif_sec_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                lpaif_spkr_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                 lpaif_ter_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                lpaif_pcm0_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",                lpaif_pcm1_clk_src.c, ""),
+	CLK_LOOKUP("core_clk_src",               lpaif_pcmoe_clk_src.c, ""),
+	CLK_LOOKUP("core_clk",               audio_core_ixfabric_clk.c, ""),
+	CLK_LOOKUP("core_clk",                  audio_wrapper_br_clk.c, ""),
+	CLK_LOOKUP("core_clk",                   q6ss_ahb_lfabif_clk.c, ""),
+	CLK_LOOKUP("core_clk",                         q6ss_ahbm_clk.c, ""),
+	CLK_LOOKUP("core_clk",                           q6ss_xo_clk.c, ""),
+	CLK_LOOKUP("core_clk",      audio_core_lpaif_pcm_data_oe_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_pri_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_pri_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",          audio_core_lpaif_pri_osr_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_pcm0_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_pcm0_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_quad_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_quad_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_quad_osr_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_sec_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_sec_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",          audio_core_lpaif_sec_osr_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_pcm1_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",        audio_core_lpaif_pcm1_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",  audio_core_lpaif_codec_spkr_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",  audio_core_lpaif_codec_spkr_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",   audio_core_lpaif_codec_spkr_osr_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_ter_ebit_clk.c, ""),
+	CLK_LOOKUP("core_clk",         audio_core_lpaif_ter_ibit_clk.c, ""),
+	CLK_LOOKUP("core_clk",          audio_core_lpaif_ter_osr_clk.c, ""),
+
+	CLK_LOOKUP("core_clk",         q6ss_xo_clk.c,  "fe200000.qcom,lpass"),
+	CLK_LOOKUP("bus_clk", gcc_lpass_q6_axi_clk.c,  "fe200000.qcom,lpass"),
+	CLK_LOOKUP("iface_clk", q6ss_ahb_lfabif_clk.c, "fe200000.qcom,lpass"),
+	CLK_LOOKUP("reg_clk",        q6ss_ahbm_clk.c,  "fe200000.qcom,lpass"),
+};
+
+static struct clk_lookup msm_clocks_8910_rumi[] = {
+	CLK_DUMMY("core_clk",   BLSP1_UART_CLK, "f991f000.serial", OFF),
+	CLK_DUMMY("iface_clk",  BLSP1_UART_CLK, "f991f000.serial", OFF),
+	CLK_DUMMY("iface_clk",  HSUSB_IFACE_CLK, "f9a55000.usb", OFF),
+	CLK_DUMMY("core_clk",	HSUSB_CORE_CLK, "f9a55000.usb", OFF),
+	CLK_DUMMY("iface_clk",	NULL,		"msm_sdcc.1", OFF),
+	CLK_DUMMY("core_clk",	NULL,		"msm_sdcc.1", OFF),
+	CLK_DUMMY("bus_clk",	NULL,		"msm_sdcc.1", OFF),
+	CLK_DUMMY("iface_clk",	NULL,		"msm_sdcc.2", OFF),
+	CLK_DUMMY("core_clk",	NULL,		"msm_sdcc.2", OFF),
+	CLK_DUMMY("bus_clk",	NULL,		"msm_sdcc.2", OFF),
+	CLK_DUMMY("dfab_clk",	DFAB_CLK,	"msm_sps", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd890000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd890000.qcom,iommu", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd860000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd860000.qcom,iommu", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd870000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd870000.qcom,iommu", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd880000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd880000.qcom,iommu", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd000000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd000000.qcom,iommu", OFF),
+	CLK_DUMMY("iface_clk",  NULL, "fd010000.qcom,iommu", OFF),
+	CLK_DUMMY("core_clk",   NULL, "fd010000.qcom,iommu", OFF),
+};
+
+struct clock_init_data msm8910_rumi_clock_init_data __initdata = {
+	.table = msm_clocks_8910_rumi,
+	.size = ARRAY_SIZE(msm_clocks_8910_rumi),
+};
+
+static struct pll_config_regs gpll0_regs __initdata = {
+	.l_reg = (void __iomem *)GPLL0_L_VAL,
+	.m_reg = (void __iomem *)GPLL0_M_VAL,
+	.n_reg = (void __iomem *)GPLL0_N_VAL,
+	.config_reg = (void __iomem *)GPLL0_USER_CTL,
+	.mode_reg = (void __iomem *)GPLL0_MODE,
+	.base = &virt_bases[GCC_BASE],
+};
+
+/* GPLL0 at 600 MHz, main output enabled. */
+static struct pll_config gpll0_config __initdata = {
+	.l = 0x1f,
+	.m = 0x1,
+	.n = 0x4,
+	.vco_val = 0x0,
+	.vco_mask = BM(21, 20),
+	.pre_div_val = 0x0,
+	.pre_div_mask = BM(14, 12),
+	.post_div_val = 0x0,
+	.post_div_mask = BM(9, 8),
+	.mn_ena_val = BIT(24),
+	.mn_ena_mask = BIT(24),
+	.main_output_val = BIT(0),
+	.main_output_mask = BIT(0),
+};
+
+/* MMPLL0 at 800 MHz, main output enabled. */
+static struct pll_config mmpll0_config __initdata = {
+	.l = 0x29,
+	.m = 0x2,
+	.n = 0x3,
+	.vco_val = 0x0,
+	.vco_mask = BM(21, 20),
+	.pre_div_val = 0x0,
+	.pre_div_mask = BM(14, 12),
+	.post_div_val = 0x0,
+	.post_div_mask = BM(9, 8),
+	.mn_ena_val = BIT(24),
+	.mn_ena_mask = BIT(24),
+	.main_output_val = BIT(0),
+	.main_output_mask = BIT(0),
+};
+
+/* MMPLL1 at 1200 MHz, main output enabled. */
+static struct pll_config mmpll1_config __initdata = {
+	.l = 0x3E,
+	.m = 0x1,
+	.n = 0x2,
+	.vco_val = 0x0,
+	.vco_mask = BM(21, 20),
+	.pre_div_val = 0x0,
+	.pre_div_mask = BM(14, 12),
+	.post_div_val = 0x0,
+	.post_div_mask = BM(9, 8),
+	.mn_ena_val = BIT(24),
+	.mn_ena_mask = BIT(24),
+	.main_output_val = BIT(0),
+	.main_output_mask = BIT(0),
+};
+
+static struct pll_config_regs lpapll0_regs __initdata = {
+	.l_reg = (void __iomem *)LPAAUDIO_PLL_L_VAL,
+	.m_reg = (void __iomem *)LPAAUDIO_PLL_M_VAL,
+	.n_reg = (void __iomem *)LPAAUDIO_PLL_N_VAL,
+	.config_reg = (void __iomem *)LPAAUDIO_PLL_USER_CTL,
+	.mode_reg = (void __iomem *)LPAAUDIO_PLL_MODE,
+	.base = &virt_bases[LPASS_BASE],
+};
+
+/* LPAPLL0 at 491.52 MHz, main output enabled. */
+static struct pll_config lpapll0_config __initdata = {
+	.l = 0x33,
+	.m = 0x1,
+	.n = 0x5,
+	.vco_val = 0x0,
+	.vco_mask = BM(21, 20),
+	.pre_div_val = BVAL(14, 12, 0x1),
+	.pre_div_mask = BM(14, 12),
+	.post_div_val = 0x0,
+	.post_div_mask = BM(9, 8),
+	.mn_ena_val = BIT(24),
+	.mn_ena_mask = BIT(24),
+	.main_output_val = BIT(0),
+	.main_output_mask = BIT(0),
+};
+
+#define PLL_AUX_OUTPUT_BIT 1
+#define PLL_AUX2_OUTPUT_BIT 2
+
+#define PWR_ON_MASK		BIT(31)
+#define EN_REST_WAIT_MASK	(0xF << 20)
+#define EN_FEW_WAIT_MASK	(0xF << 16)
+#define CLK_DIS_WAIT_MASK	(0xF << 12)
+#define SW_OVERRIDE_MASK	BIT(2)
+#define HW_CONTROL_MASK		BIT(1)
+#define SW_COLLAPSE_MASK	BIT(0)
+
+/* Wait 2^n CXO cycles between all states. Here, n=2 (4 cycles). */
+#define EN_REST_WAIT_VAL	(0x2 << 20)
+#define EN_FEW_WAIT_VAL		(0x2 << 16)
+#define CLK_DIS_WAIT_VAL	(0x2 << 12)
+#define GDSC_TIMEOUT_US		50000
+
+static void __init reg_init(void)
+{
+	u32 regval, status;
+	int ret;
+
+	if (!(readl_relaxed(GCC_REG_BASE(GPLL0_STATUS))
+			& gpll0_clk_src.status_mask))
+		configure_sr_hpm_lp_pll(&gpll0_config, &gpll0_regs, 1);
+
+	configure_sr_hpm_lp_pll(&mmpll0_config, &mmpll0_regs, 1);
+	configure_sr_hpm_lp_pll(&mmpll1_config, &mmpll1_regs, 1);
+	configure_sr_hpm_lp_pll(&lpapll0_config, &lpapll0_regs, 1);
+
+	/* Enable GPLL0's aux outputs. */
+	regval = readl_relaxed(GCC_REG_BASE(GPLL0_USER_CTL));
+	regval |= BIT(PLL_AUX_OUTPUT_BIT) | BIT(PLL_AUX2_OUTPUT_BIT);
+	writel_relaxed(regval, GCC_REG_BASE(GPLL0_USER_CTL));
+
+	/* Vote for GPLL0 to turn on. Needed by acpuclock. */
+	regval = readl_relaxed(GCC_REG_BASE(APCS_GPLL_ENA_VOTE));
+	regval |= BIT(0);
+	writel_relaxed(regval, GCC_REG_BASE(APCS_GPLL_ENA_VOTE));
+
+	/*
+	 * TODO: Confirm that no clocks need to be voted on in this sleep vote
+	 * register.
+	 */
+	writel_relaxed(0x0, GCC_REG_BASE(APCS_CLOCK_SLEEP_ENA_VOTE));
+
+	/*
+	 * TODO: The following sequence enables the LPASS audio core GDSC.
+	 * Remove when this becomes unnecessary.
+	 */
+
+	/*
+	 * Disable HW trigger: collapse/restore occur based on registers writes.
+	 * Disable SW override: Use hardware state-machine for sequencing.
+	 */
+	regval = readl_relaxed(LPASS_REG_BASE(AUDIO_CORE_GDSCR));
+	regval &= ~(HW_CONTROL_MASK | SW_OVERRIDE_MASK);
+
+	/* Configure wait time between states. */
+	regval &= ~(EN_REST_WAIT_MASK | EN_FEW_WAIT_MASK | CLK_DIS_WAIT_MASK);
+	regval |= EN_REST_WAIT_VAL | EN_FEW_WAIT_VAL | CLK_DIS_WAIT_VAL;
+	writel_relaxed(regval, LPASS_REG_BASE(AUDIO_CORE_GDSCR));
+
+	regval = readl_relaxed(LPASS_REG_BASE(AUDIO_CORE_GDSCR));
+	regval &= ~BIT(0);
+	writel_relaxed(regval, LPASS_REG_BASE(AUDIO_CORE_GDSCR));
+
+	ret = readl_poll_timeout(LPASS_REG_BASE(AUDIO_CORE_GDSCR), status,
+				status & PWR_ON_MASK, 50, GDSC_TIMEOUT_US);
+	WARN(ret, "LPASS Audio Core GDSC did not power on.\n");
+}
+
+static void __init msm8910_clock_post_init(void)
+{
+	/*
+	 * Hold an active set vote for CXO; this is because CXO is expected
+	 * to remain on whenever CPUs aren't power collapsed.
+	 */
+	clk_prepare_enable(&gcc_xo_a_clk_src.c);
+
+
+	/* Set rates for single-rate clocks. */
+	clk_set_rate(&usb_hs_system_clk_src.c,
+			usb_hs_system_clk_src.freq_tbl[0].freq_hz);
+	clk_set_rate(&pdm2_clk_src.c, pdm2_clk_src.freq_tbl[0].freq_hz);
+	clk_set_rate(&mclk0_clk_src.c, mclk0_clk_src.freq_tbl[0].freq_hz);
+	clk_set_rate(&mclk1_clk_src.c, mclk1_clk_src.freq_tbl[0].freq_hz);
+	clk_set_rate(&audio_core_slimbus_core_clk_src.c,
+			audio_core_slimbus_core_clk_src.freq_tbl[0].freq_hz);
+}
+
+#define GCC_CC_PHYS		0xFC400000
+#define GCC_CC_SIZE		SZ_16K
+
+#define MMSS_CC_PHYS		0xFD8C0000
+#define MMSS_CC_SIZE		SZ_256K
+
+#define LPASS_CC_PHYS		0xFE000000
+#define LPASS_CC_SIZE		SZ_256K
+
+#define APCS_GCC_CC_PHYS	0xF9011000
+#define APCS_GCC_CC_SIZE	SZ_4K
+
+static void __init msm8910_clock_pre_init(void)
+{
+	virt_bases[GCC_BASE] = ioremap(GCC_CC_PHYS, GCC_CC_SIZE);
+	if (!virt_bases[GCC_BASE])
+		panic("clock-8910: Unable to ioremap GCC memory!");
+
+	virt_bases[MMSS_BASE] = ioremap(MMSS_CC_PHYS, MMSS_CC_SIZE);
+	if (!virt_bases[MMSS_BASE])
+		panic("clock-8910: Unable to ioremap MMSS_CC memory!");
+
+	virt_bases[LPASS_BASE] = ioremap(LPASS_CC_PHYS, LPASS_CC_SIZE);
+	if (!virt_bases[LPASS_BASE])
+		panic("clock-8910: Unable to ioremap LPASS_CC memory!");
+
+	virt_bases[APCS_BASE] = ioremap(APCS_GCC_CC_PHYS, APCS_GCC_CC_SIZE);
+	if (!virt_bases[APCS_BASE])
+		panic("clock-8910: Unable to ioremap APCS_GCC_CC memory!");
+
+	clk_ops_local_pll.enable = sr_hpm_lp_pll_clk_enable;
+
+	vdd_dig_reg = rpm_regulator_get(NULL, "vdd_dig");
+	if (IS_ERR(vdd_dig_reg))
+		panic("clock-8910: Unable to get the vdd_dig regulator!");
+
+	/*
+	 * TODO: Set a voltage and enable vdd_dig, leaving the voltage high
+	 * until late_init. This may not be necessary with clock handoff;
+	 * Investigate this code on a real non-simulator target to determine
+	 * its necessity.
+	 */
+	vote_vdd_level(&vdd_dig, VDD_DIG_HIGH);
+	rpm_regulator_enable(vdd_dig_reg);
+
+	enable_rpm_scaling();
+
+	/* Enable a clock to allow access to MMSS clock registers */
+	clk_prepare_enable(&gcc_mmss_noc_cfg_ahb_clk.c),
+
+	reg_init();
+
+	/* TODO: Remove this once the bus driver is in place */
+	clk_set_rate(&ahb_clk_src.c,  40000000);
+	clk_set_rate(&axi_clk_src.c, 200000000);
+	clk_prepare_enable(&mmss_mmssnoc_ahb_clk.c);
+	clk_prepare_enable(&mmss_s0_axi_clk.c);
+
+	/* TODO: Temporarily enable a clock to allow access to LPASS core
+	 * registers.
+	 */
+	clk_prepare_enable(&audio_core_ixfabric_clk.c);
+}
+
+static int __init msm8910_clock_late_init(void)
+{
+	return unvote_vdd_level(&vdd_dig, VDD_DIG_HIGH);
+}
+
+struct clock_init_data msm8910_clock_init_data __initdata = {
+	.table = msm_clocks_8910,
+	.size = ARRAY_SIZE(msm_clocks_8910),
+	.pre_init = msm8910_clock_pre_init,
+	.post_init = msm8910_clock_post_init,
+	.late_init = msm8910_clock_late_init,
+};
diff --git a/arch/arm/mach-msm/clock-8960.c b/arch/arm/mach-msm/clock-8960.c
index f6386b4..7038b06 100644
--- a/arch/arm/mach-msm/clock-8960.c
+++ b/arch/arm/mach-msm/clock-8960.c
@@ -5459,7 +5459,18 @@
 	CLK_LOOKUP("mem_iface_clk",	imem_p_clk.c,	"kgsl-3d0.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,		"mdp.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,	"footswitch-8x60.4"),
-	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,		"msm_iommu"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.0"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.1"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.2"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.3"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.4"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.5"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.6"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.7"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.8"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.9"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.10"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.11"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"msm_rotator.0"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"footswitch-8x60.6"),
 	CLK_LOOKUP("iface_clk",		vcodec_p_clk.c,		"msm_vidc.0"),
@@ -5807,7 +5818,16 @@
 	CLK_LOOKUP("mem_iface_clk",	imem_p_clk.c,	"kgsl-3d0.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,		"mdp.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,	"footswitch-8x60.4"),
-	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.0"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.1"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.2"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.3"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.4"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.5"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.6"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.7"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.8"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.9"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"msm_rotator.0"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"footswitch-8x60.6"),
 	CLK_LOOKUP("iface_clk",		vcodec_p_clk.c,	"msm_vidc.0"),
@@ -5902,12 +5922,15 @@
 	CLK_LOOKUP("iface_clk",		gfx2d0_p_clk.c,	"footswitch-8x60.0"),
 	CLK_LOOKUP("iface_clk",		gfx2d1_p_clk.c,	"kgsl-2d1.1"),
 	CLK_LOOKUP("iface_clk",		gfx2d1_p_clk.c,	"footswitch-8x60.1"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,		"msm_iommu.10"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,		"msm_iommu.11"),
 	CLK_LOOKUP("core_clk",		gfx2d0_clk.c,		"msm_iommu.10"),
 	CLK_LOOKUP("core_clk",		gfx2d1_clk.c,		"msm_iommu.11"),
 };
 
 static struct clk_lookup msm_clocks_8960ab_only[] __initdata = {
 	CLK_LOOKUP("bus_clk", gfx3d_axi_clk.c, "footswitch-8x60.2"),
+	CLK_LOOKUP("iface_clk",	smmu_p_clk.c,	"msm_iommu.10"),
 	CLK_LOOKUP("core_clk", gfx3d_axi_clk.c, "msm_iommu.10"),
 	CLK_LOOKUP("div_clk",	tv_src_div_clk.c,	""),
 };
@@ -6144,7 +6167,17 @@
 	CLK_LOOKUP("mem_iface_clk",	imem_p_clk.c,	"kgsl-3d0.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,		"mdp.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,	"footswitch-8x60.4"),
-	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.0"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.1"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.2"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.3"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.4"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.5"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.6"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.7"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.8"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.9"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.10"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"msm_rotator.0"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"footswitch-8x60.6"),
 	CLK_LOOKUP("iface_clk",		vcodec_p_clk.c,	"msm_vidc.0"),
diff --git a/arch/arm/mach-msm/clock-8974.c b/arch/arm/mach-msm/clock-8974.c
index 95f9327..760587f 100644
--- a/arch/arm/mach-msm/clock-8974.c
+++ b/arch/arm/mach-msm/clock-8974.c
@@ -666,7 +666,8 @@
 #define A1_ID		 4
 #define A2_ID		 5
 #define DIFF_CLK_ID	 7
-#define DIV_CLK_ID	11
+#define DIV_CLK1_ID	11
+#define DIV_CLK2_ID	12
 
 DEFINE_CLK_RPM_SMD(pnoc_clk, pnoc_a_clk, RPM_BUS_CLK_TYPE, PNOC_ID, NULL);
 DEFINE_CLK_RPM_SMD(snoc_clk, snoc_a_clk, RPM_BUS_CLK_TYPE, SNOC_ID, NULL);
@@ -689,7 +690,8 @@
 DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a0, cxo_a0_a, A0_ID);
 DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a1, cxo_a1_a, A1_ID);
 DEFINE_CLK_RPM_SMD_XO_BUFFER(cxo_a2, cxo_a2_a, A2_ID);
-DEFINE_CLK_RPM_SMD_XO_BUFFER(div_clk, div_a_clk, DIV_CLK_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(div_clk1, div_a_clk1, DIV_CLK1_ID);
+DEFINE_CLK_RPM_SMD_XO_BUFFER(div_clk2, div_a_clk2, DIV_CLK2_ID);
 DEFINE_CLK_RPM_SMD_XO_BUFFER(diff_clk, diff_a_clk, DIFF_CLK_ID);
 
 DEFINE_CLK_RPM_SMD_XO_BUFFER_PINCTRL(cxo_d0_pin, cxo_d0_a_pin, D0_ID);
@@ -2324,6 +2326,7 @@
 
 static struct branch_clk gcc_usb_hsic_system_clk = {
 	.cbcr_reg = USB_HSIC_SYSTEM_CBCR,
+	.bcr_reg = USB_HS_HSIC_BCR,
 	.base = &virt_bases[GCC_BASE],
 	.c = {
 		.parent = &usb_hsic_system_clk_src.c,
@@ -3018,34 +3021,17 @@
 
 static int hdmi_pll_clk_enable(struct clk *c)
 {
-	int ret;
-	unsigned long flags;
-
-	spin_lock_irqsave(&local_clock_reg_lock, flags);
-	ret = hdmi_pll_enable();
-	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
-	return ret;
+	return hdmi_pll_enable();
 }
 
 static void hdmi_pll_clk_disable(struct clk *c)
 {
-	unsigned long flags;
-
-	spin_lock_irqsave(&local_clock_reg_lock, flags);
 	hdmi_pll_disable();
-	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
 }
 
 static int hdmi_pll_clk_set_rate(struct clk *c, unsigned long rate)
 {
-	unsigned long flags;
-	int rc;
-
-	spin_lock_irqsave(&local_clock_reg_lock, flags);
-	rc = hdmi_pll_set_rate(rate);
-	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
-
-	return rc;
+	return hdmi_pll_set_rate(rate);
 }
 
 static struct clk_ops clk_ops_hdmi_pll = {
@@ -3082,21 +3068,39 @@
  * Unlike other clocks, the HDMI rate is adjusted through PLL
  * re-programming. It is also routed through an HID divider.
  */
-static void set_rate_hdmi(struct rcg_clk *rcg, struct clk_freq_tbl *nf)
+static int rcg_clk_set_rate_hdmi(struct clk *c, unsigned long rate)
 {
-	clk_set_rate(nf->src_clk, nf->freq_hz);
+	struct clk_freq_tbl *nf;
+	struct rcg_clk *rcg = to_rcg_clk(c);
+	int rc;
+
+	for (nf = rcg->freq_tbl; nf->freq_hz != rate; nf++)
+		if (nf->freq_hz == FREQ_END) {
+			rc = -EINVAL;
+			goto out;
+		}
+
+	rc = clk_set_rate(nf->src_clk, rate);
+	if (rc < 0)
+		goto out;
 	set_rate_hid(rcg, nf);
+
+	rcg->current_freq = nf;
+	c->parent = nf->src_clk;
+out:
+	return rc;
 }
 
+static struct clk_ops clk_ops_rcg_hdmi;
+
 static struct rcg_clk extpclk_clk_src = {
 	.cmd_rcgr_reg = EXTPCLK_CMD_RCGR,
-	.set_rate = set_rate_hdmi,
 	.freq_tbl = ftbl_mdss_extpclk_clk,
 	.current_freq = &rcg_dummy_freq,
 	.base = &virt_bases[MMSS_BASE],
 	.c = {
 		.dbg_name = "extpclk_clk_src",
-		.ops = &clk_ops_rcg,
+		.ops = &clk_ops_rcg_hdmi,
 		VDD_DIG_FMAX_MAP2(LOW, 148500000, NOMINAL, 297000000),
 		CLK_INIT(extpclk_clk_src.c),
 	},
@@ -4989,7 +4993,7 @@
 	CLK_LOOKUP("iface_clk", gcc_sdcc4_ahb_clk.c, "msm_sdcc.4"),
 	CLK_LOOKUP("core_clk", gcc_sdcc4_apps_clk.c, "msm_sdcc.4"),
 	CLK_DUMMY("xo",		XO_CLK,		NULL,	OFF),
-	CLK_DUMMY("xo",		XO_CLK,		"pil_pronto",		OFF),
+	CLK_DUMMY("xo",		XO_CLK,		"fb21b000.qcom,pronto", OFF),
 	CLK_DUMMY("core_clk",	BLSP2_UART_CLK,	"f991f000.serial",	OFF),
 	CLK_DUMMY("iface_clk",	BLSP2_UART_CLK,	"f991f000.serial",	OFF),
 	CLK_DUMMY("core_clk",	SDC1_CLK,	NULL,			OFF),
@@ -5018,10 +5022,10 @@
 
 static struct clk_lookup msm_clocks_8974[] = {
 	CLK_LOOKUP("xo",	cxo_clk_src.c,	"msm_otg"),
-	CLK_LOOKUP("xo",	cxo_clk_src.c,	"pil-q6v5-lpass"),
-	CLK_LOOKUP("xo",	cxo_clk_src.c,	"pil-q6v5-mss"),
+	CLK_LOOKUP("xo",	cxo_clk_src.c,	"fe200000.qcom,lpass"),
+	CLK_LOOKUP("xo",	cxo_clk_src.c,	"fc880000.qcom,mss"),
 	CLK_LOOKUP("xo",	cxo_clk_src.c,	"fb000000.qcom,wcnss-wlan"),
-	CLK_LOOKUP("xo",	cxo_clk_src.c,	"pil_pronto"),
+	CLK_LOOKUP("xo",	cxo_clk_src.c,	"fb21b000.qcom,pronto"),
 	CLK_LOOKUP("measure",	measure_clk.c,	"debug"),
 
 	CLK_LOOKUP("dma_bam_pclk", gcc_bam_dma_ahb_clk.c, "msm_sps"),
@@ -5129,6 +5133,7 @@
 	CLK_LOOKUP("phy_clk", gcc_usb_hsic_clk.c,	  "msm_hsic_host"),
 	CLK_LOOKUP("cal_clk", gcc_usb_hsic_io_cal_clk.c,  "msm_hsic_host"),
 	CLK_LOOKUP("core_clk", gcc_usb_hsic_system_clk.c, "msm_hsic_host"),
+	CLK_LOOKUP("ref_clk", div_clk2.c, "msm_smsc_hub"),
 
 	/* Multimedia clocks */
 	CLK_LOOKUP("bus_clk_src", axi_clk_src.c, ""),
@@ -5137,11 +5142,11 @@
 	CLK_LOOKUP("pixel_clk", mdss_edppixel_clk.c, "fd923400.qcom,mdss_edp"),
 	CLK_LOOKUP("link_clk", mdss_edplink_clk.c, "fd923400.qcom,mdss_edp"),
 	CLK_LOOKUP("byte_clk", mdss_byte0_clk.c, "fd922800.qcom,mdss_dsi"),
-	CLK_LOOKUP("byte_clk", mdss_byte1_clk.c, ""),
+	CLK_LOOKUP("byte_clk", mdss_byte1_clk.c, "fd922e00.qcom,mdss_dsi"),
 	CLK_LOOKUP("core_clk", mdss_esc0_clk.c, "fd922800.qcom,mdss_dsi"),
-	CLK_LOOKUP("core_clk", mdss_esc1_clk.c, ""),
+	CLK_LOOKUP("core_clk", mdss_esc1_clk.c, "fd922e00.qcom,mdss_dsi"),
 	CLK_LOOKUP("pixel_clk", mdss_pclk0_clk.c, "fd922800.qcom,mdss_dsi"),
-	CLK_LOOKUP("pixel_clk", mdss_pclk1_clk.c, ""),
+	CLK_LOOKUP("pixel_clk", mdss_pclk1_clk.c, "fd922e00.qcom,mdss_dsi"),
 	CLK_LOOKUP("iface_clk", mdss_ahb_clk.c, "fd922100.qcom,hdmi_tx"),
 	CLK_LOOKUP("alt_iface_clk", mdss_hdmi_ahb_clk.c,
 		"fd922100.qcom,hdmi_tx"),
@@ -5367,14 +5372,14 @@
 						"msm-dai-q6.4106"),
 	CLK_LOOKUP("br_clk", audio_wrapper_br_clk.c, "fdd00000.qcom,ocmem"),
 
-	CLK_LOOKUP("bus_clk", gcc_mss_q6_bimc_axi_clk.c, "pil-q6v5-mss"),
-	CLK_LOOKUP("iface_clk", gcc_mss_cfg_ahb_clk.c, "pil-q6v5-mss"),
-	CLK_LOOKUP("mem_clk", gcc_boot_rom_ahb_clk.c,  "pil-q6v5-mss"),
+	CLK_LOOKUP("bus_clk", gcc_mss_q6_bimc_axi_clk.c, "fc880000.qcom,mss"),
+	CLK_LOOKUP("iface_clk", gcc_mss_cfg_ahb_clk.c, "fc880000.qcom,mss"),
+	CLK_LOOKUP("mem_clk", gcc_boot_rom_ahb_clk.c,  "fc880000.qcom,mss"),
 
-	CLK_LOOKUP("core_clk",         q6ss_xo_clk.c,  "pil-q6v5-lpass"),
-	CLK_LOOKUP("bus_clk", gcc_lpass_q6_axi_clk.c,  "pil-q6v5-lpass"),
-	CLK_LOOKUP("iface_clk", q6ss_ahb_lfabif_clk.c, "pil-q6v5-lpass"),
-	CLK_LOOKUP("reg_clk",        q6ss_ahbm_clk.c,  "pil-q6v5-lpass"),
+	CLK_LOOKUP("core_clk",         q6ss_xo_clk.c,  "fe200000.qcom,lpass"),
+	CLK_LOOKUP("bus_clk", gcc_lpass_q6_axi_clk.c,  "fe200000.qcom,lpass"),
+	CLK_LOOKUP("iface_clk", q6ss_ahb_lfabif_clk.c, "fe200000.qcom,lpass"),
+	CLK_LOOKUP("reg_clk",        q6ss_ahbm_clk.c,  "fe200000.qcom,lpass"),
 	CLK_LOOKUP("core_clk", gcc_prng_ahb_clk.c, "msm_rng"),
 
 	CLK_LOOKUP("dfab_clk", pnoc_sps_clk.c, "msm_sps"),
@@ -5684,6 +5689,9 @@
 	clk_ops_pixel = clk_ops_rcg;
 	clk_ops_pixel.set_rate = set_rate_pixel;
 
+	clk_ops_rcg_hdmi = clk_ops_rcg;
+	clk_ops_rcg_hdmi.set_rate = rcg_clk_set_rate_hdmi;
+
 	mdss_clk_ctrl_init();
 }
 
diff --git a/arch/arm/mach-msm/clock-8x60.c b/arch/arm/mach-msm/clock-8x60.c
index 2ad425d..dade0ed 100644
--- a/arch/arm/mach-msm/clock-8x60.c
+++ b/arch/arm/mach-msm/clock-8x60.c
@@ -3689,7 +3689,18 @@
 	CLK_LOOKUP("mem_iface_clk",	imem_p_clk.c,	"kgsl-3d0.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,		"mdp.0"),
 	CLK_LOOKUP("iface_clk",		mdp_p_clk.c,	"footswitch-8x60.4"),
-	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.0"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.1"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.2"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.3"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.4"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.5"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.6"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.7"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.8"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.9"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.10"),
+	CLK_LOOKUP("iface_clk",		smmu_p_clk.c,	"msm_iommu.11"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"msm_rotator.0"),
 	CLK_LOOKUP("iface_clk",		rot_p_clk.c,	"footswitch-8x60.6"),
 	CLK_LOOKUP("tv_enc_pclk",	tv_enc_p_clk.c,		NULL),
diff --git a/arch/arm/mach-msm/clock-local2.c b/arch/arm/mach-msm/clock-local2.c
index 5923951..cf42355 100644
--- a/arch/arm/mach-msm/clock-local2.c
+++ b/arch/arm/mach-msm/clock-local2.c
@@ -461,13 +461,28 @@
 
 static int branch_clk_list_rate(struct clk *c, unsigned n)
 {
+	int level, fmax = 0, rate;
 	struct branch_clk *branch = to_branch_clk(c);
+	struct clk *parent = c->parent;
 
 	if (branch->has_sibling == 1)
 		return -ENXIO;
 
-	if (c->parent && c->parent->ops->list_rate)
-		return c->parent->ops->list_rate(c->parent, n);
+	if (!parent || !parent->ops->list_rate)
+		return -ENXIO;
+
+	/* Find max frequency supported within voltage constraints. */
+	if (!parent->vdd_class) {
+		fmax = INT_MAX;
+	} else {
+		for (level = 0; level < parent->num_fmax; level++)
+			if (parent->fmax[level])
+				fmax = parent->fmax[level];
+	}
+
+	rate = parent->ops->list_rate(parent, n);
+	if (rate <= fmax)
+		return rate;
 	else
 		return -ENXIO;
 }
diff --git a/arch/arm/mach-msm/clock-mdss-8974.c b/arch/arm/mach-msm/clock-mdss-8974.c
index c30e566..79bc639 100644
--- a/arch/arm/mach-msm/clock-mdss-8974.c
+++ b/arch/arm/mach-msm/clock-mdss-8974.c
@@ -350,9 +350,11 @@
 
 void hdmi_pll_disable(void)
 {
+	clk_enable(mdss_dsi_ahb_clk);
 	REG_W(0x0, hdmi_phy_pll_base + HDMI_UNI_PLL_GLB_CFG);
 	udelay(5);
 	REG_W(0x0, hdmi_phy_base + HDMI_PHY_GLB_CFG);
+	clk_disable(mdss_dsi_ahb_clk);
 
 	hdmi_pll_on = 0;
 } /* hdmi_pll_disable */
@@ -362,6 +364,7 @@
 	u32 status;
 	u32 max_reads, timeout_us;
 
+	clk_enable(mdss_dsi_ahb_clk);
 	/* Global Enable */
 	REG_W(0x81, hdmi_phy_base + HDMI_PHY_GLB_CFG);
 	/* Power up power gen */
@@ -387,6 +390,7 @@
 		pr_err("%s: hdmi phy pll status=%x failed to Lock\n",
 		       __func__, status);
 		hdmi_pll_disable();
+		clk_disable(mdss_dsi_ahb_clk);
 		return -EINVAL;
 	}
 	pr_debug("%s: hdmi phy pll is locked\n", __func__);
@@ -400,9 +404,11 @@
 		pr_err("%s: hdmi phy status=%x failed to Lock\n",
 		       __func__, status);
 		hdmi_pll_disable();
+		clk_disable(mdss_dsi_ahb_clk);
 		return -EINVAL;
 	}
 	pr_debug("%s: hdmi phy is locked\n", __func__);
+	clk_disable(mdss_dsi_ahb_clk);
 
 	hdmi_pll_on = 1;
 
@@ -418,6 +424,7 @@
 		set_power_dwn = 1;
 	}
 
+	clk_enable(mdss_dsi_ahb_clk);
 	pr_debug("%s: rate=%ld\n", __func__, rate);
 	switch (rate) {
 	case 0:
@@ -746,6 +753,8 @@
 	/* Make sure writes complete before disabling iface clock */
 	mb();
 
+	clk_disable(mdss_dsi_ahb_clk);
+
 	if (set_power_dwn)
 		hdmi_pll_enable();
 
diff --git a/arch/arm/mach-msm/clock-pll.c b/arch/arm/mach-msm/clock-pll.c
index 8e11d37..cd4ead1 100644
--- a/arch/arm/mach-msm/clock-pll.c
+++ b/arch/arm/mach-msm/clock-pll.c
@@ -54,6 +54,7 @@
 static DEFINE_SPINLOCK(pll_reg_lock);
 
 #define ENABLE_WAIT_MAX_LOOPS 200
+#define PLL_LOCKED_BIT BIT(16)
 
 static int pll_vote_clk_enable(struct clk *c)
 {
@@ -148,6 +149,51 @@
 	writel_relaxed(regval, pll_config);
 }
 
+static int sr2_pll_clk_enable(struct clk *c)
+{
+	unsigned long flags;
+	struct pll_clk *pll = to_pll_clk(c);
+	int ret = 0, count;
+	u32 mode = readl_relaxed(PLL_MODE_REG(pll));
+
+	spin_lock_irqsave(&pll_reg_lock, flags);
+
+	/* Disable PLL bypass mode. */
+	mode |= PLL_BYPASSNL;
+	writel_relaxed(mode, PLL_MODE_REG(pll));
+
+	/*
+	 * H/W requires a 5us delay between disabling the bypass and
+	 * de-asserting the reset. Delay 10us just to be safe.
+	 */
+	mb();
+	udelay(10);
+
+	/* De-assert active-low PLL reset. */
+	mode |= PLL_RESET_N;
+	writel_relaxed(mode, PLL_MODE_REG(pll));
+
+	/* Wait for pll to lock. */
+	for (count = ENABLE_WAIT_MAX_LOOPS; count > 0; count--) {
+		if (readl_relaxed(PLL_STATUS_REG(pll)) & PLL_LOCKED_BIT)
+			break;
+		udelay(1);
+	}
+
+	if (!(readl_relaxed(PLL_STATUS_REG(pll)) & PLL_LOCKED_BIT))
+		pr_err("PLL %s didn't lock after enabling it!\n", c->dbg_name);
+
+	/* Enable PLL output. */
+	mode |= PLL_OUTCTRL;
+	writel_relaxed(mode, PLL_MODE_REG(pll));
+
+	/* Ensure that the write above goes through before returning. */
+	mb();
+
+	spin_unlock_irqrestore(&pll_reg_lock, flags);
+	return ret;
+}
+
 static void __pll_clk_enable_reg(void __iomem *mode_reg)
 {
 	u32 mode = readl_relaxed(mode_reg);
@@ -290,8 +336,6 @@
 	return 0;
 }
 
-#define PLL_LOCKED_BIT BIT(16)
-
 int sr_hpm_lp_pll_clk_enable(struct clk *c)
 {
 	unsigned long flags;
@@ -337,6 +381,13 @@
 	.handoff = local_pll_clk_handoff,
 };
 
+struct clk_ops clk_ops_sr2_pll = {
+	.enable = sr2_pll_clk_enable,
+	.disable = local_pll_clk_disable,
+	.set_rate = local_pll_clk_set_rate,
+	.handoff = local_pll_clk_handoff,
+};
+
 struct pll_rate {
 	unsigned int lvalue;
 	unsigned long rate;
diff --git a/arch/arm/mach-msm/clock-pll.h b/arch/arm/mach-msm/clock-pll.h
index cb334d7..823103a 100644
--- a/arch/arm/mach-msm/clock-pll.h
+++ b/arch/arm/mach-msm/clock-pll.h
@@ -160,6 +160,7 @@
 };
 
 extern struct clk_ops clk_ops_local_pll;
+extern struct clk_ops clk_ops_sr2_pll;
 
 static inline struct pll_clk *to_pll_clk(struct clk *c)
 {
diff --git a/arch/arm/mach-msm/clock-rpm.c b/arch/arm/mach-msm/clock-rpm.c
index 63e67b3..a4def28 100644
--- a/arch/arm/mach-msm/clock-rpm.c
+++ b/arch/arm/mach-msm/clock-rpm.c
@@ -78,6 +78,13 @@
 
 static int clk_rpmrs_handoff_smd(struct rpm_clk *r)
 {
+	if (!r->branch) {
+		r->last_set_khz = INT_MAX;
+		if (!r->active_only)
+			r->last_set_sleep_khz = INT_MAX;
+		r->c.rate = 1 * r->factor;
+	}
+
 	return 0;
 }
 
diff --git a/arch/arm/mach-msm/clock.h b/arch/arm/mach-msm/clock.h
index 8a75d390..181cf4c 100644
--- a/arch/arm/mach-msm/clock.h
+++ b/arch/arm/mach-msm/clock.h
@@ -49,6 +49,8 @@
 extern struct clock_init_data msm8930_pm8917_clock_init_data;
 extern struct clock_init_data msm8974_clock_init_data;
 extern struct clock_init_data msm8974_rumi_clock_init_data;
+extern struct clock_init_data msm8910_clock_init_data;
+extern struct clock_init_data msm8910_rumi_clock_init_data;
 
 int msm_clock_init(struct clock_init_data *data);
 int find_vdd_level(struct clk *clk, unsigned long rate);
diff --git a/arch/arm/mach-msm/cpufreq.c b/arch/arm/mach-msm/cpufreq.c
index e0d98b7..d862d6d 100644
--- a/arch/arm/mach-msm/cpufreq.c
+++ b/arch/arm/mach-msm/cpufreq.c
@@ -227,7 +227,7 @@
 	 * be changed independently. Each cpu is bound to
 	 * same frequency. Hence set the cpumask to all cpu.
 	 */
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		cpumask_setall(policy->cpus);
 
 	if (cpufreq_frequency_table_cpuinfo(policy, table)) {
diff --git a/arch/arm/mach-msm/devices-8064.c b/arch/arm/mach-msm/devices-8064.c
index 8d10d6a..dcd90d0 100644
--- a/arch/arm/mach-msm/devices-8064.c
+++ b/arch/arm/mach-msm/devices-8064.c
@@ -19,6 +19,8 @@
 #include <linux/clkdev.h>
 #include <linux/dma-mapping.h>
 #include <linux/coresight.h>
+#include <linux/avtimer.h>
+#include <linux/ahci_platform.h>
 #include <mach/irqs-8064.h>
 #include <mach/board.h>
 #include <mach/msm_iomap.h>
@@ -103,6 +105,9 @@
 #define MSM8064_PC_CNTR_PHYS	(APQ8064_IMEM_PHYS + 0x664)
 #define MSM8064_PC_CNTR_SIZE		0x40
 #define MSM8064_RPM_MASTER_STATS_BASE	0x10BB00
+/* avtimer */
+#define AVTIMER_MSW_PHYSICAL_ADDRESS 0x2800900C
+#define AVTIMER_LSW_PHYSICAL_ADDRESS 0x28009008
 
 static struct resource msm8064_resources_pccntr[] = {
 	{
@@ -1773,6 +1778,48 @@
 	return platform_device_register(pdev);
 }
 
+#define MSM_SATA_AHCI_BASE	0x29000000
+#define MSM_SATA_AHCI_REGS_SZ	0x17C
+
+static struct resource resources_ahci[] = {
+	{
+		.name	= "ahci_mem",
+		.flags	= IORESOURCE_MEM,
+		.start	= MSM_SATA_AHCI_BASE,
+		.end	= MSM_SATA_AHCI_BASE + MSM_SATA_AHCI_REGS_SZ - 1,
+	},
+	{
+		.name	= "ahci_irq",
+		.flags	= IORESOURCE_IRQ,
+		.start	= SATA_CONTROLLER_IRQ,
+		.end	= SATA_CONTROLLER_IRQ,
+	},
+};
+
+static u64 ahci_dma_mask = DMA_BIT_MASK(32);
+static struct platform_device apq8064_device_ahci = {
+	.name		= "ahci",
+	.id		= 0,
+	.num_resources	= ARRAY_SIZE(resources_ahci),
+	.resource	= resources_ahci,
+	.dev		= {
+		.dma_mask		= &ahci_dma_mask,
+		.coherent_dma_mask	= DMA_BIT_MASK(32),
+	},
+};
+
+int __init apq8064_add_ahci(struct ahci_platform_data *platd)
+{
+	struct platform_device	*pdev;
+
+	if (!platd)
+		return -EINVAL;
+
+	pdev = &apq8064_device_ahci;
+	pdev->dev.platform_data = platd;
+	return platform_device_register(pdev);
+}
+
 static struct resource resources_sps[] = {
 	{
 		.name	= "pipe_mem",
@@ -2792,7 +2839,7 @@
 		.slack_weight_thresh_pct	= 3,
 		.slack_time_min_us		= 45000,
 		.slack_time_max_us		= 45000,
-		.ss_iobusy_conv			= 100,
+		.ss_no_corr_below_freq		= 0,
 		.ss_win_size_min_us		= 1000000,
 		.ss_win_size_max_us		= 1000000,
 		.ss_util_pct			= 95,
@@ -3292,3 +3339,8 @@
 		.platform_data = &apq8064_cache_dump_pdata,
 	},
 };
+
+struct dev_avtimer_data dev_avtimer_pdata = {
+	.avtimer_msw_phy_addr = AVTIMER_MSW_PHYSICAL_ADDRESS,
+	.avtimer_lsw_phy_addr = AVTIMER_LSW_PHYSICAL_ADDRESS,
+};
diff --git a/arch/arm/mach-msm/devices-8960.c b/arch/arm/mach-msm/devices-8960.c
index 2421646..4780c57 100644
--- a/arch/arm/mach-msm/devices-8960.c
+++ b/arch/arm/mach-msm/devices-8960.c
@@ -3030,7 +3030,7 @@
 		.ss_win_size_min_us		= 1000000,
 		.ss_win_size_max_us		= 1000000,
 		.ss_util_pct			= 95,
-		.ss_iobusy_conv			= 100,
+		.ss_no_corr_below_freq		= 0,
 	},
 	.energy_coeffs	= {
 		.active_coeff_a		= 2492,
@@ -3067,7 +3067,7 @@
 		.ss_win_size_min_us		= 1000000,
 		.ss_win_size_max_us		= 1000000,
 		.ss_util_pct			= 95,
-		.ss_iobusy_conv			= 100,
+		.ss_no_corr_below_freq		= 0,
 	},
 	.energy_coeffs	= {
 		.active_coeff_a		= 2492,
@@ -4430,9 +4430,19 @@
 	},
 };
 
+static struct resource msm_gpio_resources[] = {
+	{
+		.start	= TLMM_MSM_SUMMARY_IRQ,
+		.end	= TLMM_MSM_SUMMARY_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
 struct platform_device msm_gpio_device = {
 	.name = "msmgpio",
 	.id = -1,
+	.num_resources	= ARRAY_SIZE(msm_gpio_resources),
+	.resource	= msm_gpio_resources,
 };
 
 struct platform_device mdm_sglte_device = {
diff --git a/arch/arm/mach-msm/devices-9615.c b/arch/arm/mach-msm/devices-9615.c
index e55e9a7..fc65cb7 100644
--- a/arch/arm/mach-msm/devices-9615.c
+++ b/arch/arm/mach-msm/devices-9615.c
@@ -1506,9 +1506,19 @@
 	},
 };
 
+static struct resource msm_gpio_resources[] = {
+	{
+		.start	= TLMM_MSM_SUMMARY_IRQ,
+		.end	= TLMM_MSM_SUMMARY_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
 struct platform_device msm_gpio_device = {
 	.name = "msmgpio",
 	.id = -1,
+	.num_resources	= ARRAY_SIZE(msm_gpio_resources),
+	.resource	= msm_gpio_resources,
 };
 
 void __init msm9615_device_init(void)
diff --git a/arch/arm/mach-msm/devices-msm7x27a.c b/arch/arm/mach-msm/devices-msm7x27a.c
index 8fc5020..2d9872e 100644
--- a/arch/arm/mach-msm/devices-msm7x27a.c
+++ b/arch/arm/mach-msm/devices-msm7x27a.c
@@ -36,6 +36,7 @@
 #include "devices-msm7x2xa.h"
 #include "footswitch.h"
 #include "acpuclock.h"
+#include "acpuclock-8625q.h"
 #include "spm.h"
 #include "mpm-8625.h"
 #include "irq.h"
@@ -240,6 +241,21 @@
 	.dev.platform_data = &msm7x27aa_acpuclk_pdata,
 };
 
+static struct acpuclk_pdata msm8625q_pdata = {
+	.max_speed_delta_khz = 801600,
+};
+
+static struct acpuclk_pdata_8625q msm8625q_acpuclk_pdata = {
+	.acpu_clk_data = &msm8625q_pdata,
+	.pvs_voltage_uv = 1350000,
+};
+
+struct platform_device msm8625q_device_acpuclk = {
+	.name		= "acpuclock-8625q",
+	.id		= -1,
+	.dev.platform_data = &msm8625q_acpuclk_pdata,
+};
+
 static struct acpuclk_pdata msm8625_acpuclk_pdata = {
 	/* TODO: Need to update speed delta from H/w Team */
 	.max_speed_delta_khz = 604800,
@@ -515,7 +531,7 @@
 
 void __init msm_pm_register_irqs(void)
 {
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		msm_pm_set_irq_extns(&msm8625_pm_irq_calls);
 	else
 		msm_pm_set_irq_extns(&msm7x27a_pm_irq_calls);
@@ -530,8 +546,9 @@
 void __init msm_pm_register_cpr_ops(void)
 {
 	/* CPR presents on revision >= v2.0 chipsets */
-	if (cpu_is_msm8625() &&
+	if ((cpu_is_msm8625() &&
 			SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 2)
+			|| cpu_is_msm8625q())
 		msm_pm_set_cpr_ops(&msm8625_pm_cpr_ops);
 }
 
@@ -952,7 +969,7 @@
 
 void __init msm8x25_kgsl_3d0_init(void)
 {
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		kgsl_3d0_pdata.idle_timeout = HZ/5;
 		kgsl_3d0_pdata.strtstp_sleepwake = false;
 
@@ -1406,7 +1423,7 @@
 	if (controller < 1 || controller > 4)
 		return -EINVAL;
 
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		pdev = msm8625_sdcc_devices[controller-1];
 	else
 		pdev = msm_sdcc_devices[controller-1];
@@ -1495,7 +1512,7 @@
 {
 	struct platform_device	*pdev;
 
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		pdev = msm8625_host_devices[host];
 	else
 		pdev = msm_host_devices[host];
@@ -1598,12 +1615,12 @@
 void __init msm_fb_register_device(char *name, void *data)
 {
 	if (!strncmp(name, "mdp", 3)) {
-		if (cpu_is_msm8625())
+		if (cpu_is_msm8625() || cpu_is_msm8625q())
 			msm_register_device(&msm8625_mdp_device, data);
 		else
 			msm_register_device(&msm_mdp_device, data);
 	} else if (!strncmp(name, "mipi_dsi", 8)) {
-		if (cpu_is_msm8625()) {
+		if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 			msm_register_device(&msm8625_mipi_dsi_device, data);
 			mipi_dsi_device = msm8625_mipi_dsi_device;
 		} else {
@@ -1774,13 +1791,6 @@
 	},
 };
 
-struct msm_cpr_vp_data vp_data = {
-	.min_volt = 1000000,
-	.max_volt = 1350000,
-	.default_volt = 1300000,
-	.step_size = 12500,
-};
-
 static uint32_t
 msm_cpr_get_quot(uint32_t max_quot, uint32_t max_freq, uint32_t new_freq)
 {
@@ -1822,7 +1832,7 @@
 	.max_freq = 1401600,
 	.max_quot = 0,
 	.disable_cpr = false,
-	.vp_data = &vp_data,
+	.step_size = 12500,
 	.get_quot = msm_cpr_get_quot,
 	.clk_enable = msm_cpr_clk_enable,
 };
@@ -1929,6 +1939,8 @@
 	else if (msm8625_cpu_id() == MSM8625)
 		msm_cpr_pdata.max_freq = 1008000;
 
+	if (machine_is_qrd_skud_prime() || cpu_is_msm8625q())
+		msm_cpr_pdata.step_size = 6250;
 	platform_device_register(&msm8625_vp_device);
 	platform_device_register(&msm8625_device_cpr);
 }
@@ -2035,6 +2047,24 @@
 	return ret;
 }
 
+static int __init msm_acpuclock_init(int nominal_voltage,
+					int default_turbo_voltage)
+{
+	struct cpr_info_type *acpu_info = NULL;
+	acpu_info = kzalloc(sizeof(struct cpr_info_type), GFP_KERNEL);
+	if (!acpu_info) {
+		pr_err("%s: Out of memory %d\n", __func__, -ENOMEM);
+		return -ENOMEM;
+	}
+	msm_smem_get_cpr_info(acpu_info);
+	msm8625q_acpuclk_pdata.pvs_voltage_uv =
+			msm_c2_pmic_mv[acpu_info->pvs_fuse & 0x1F];
+	kfree(acpu_info);
+	msm8625q_acpuclk_pdata.nominal_voltage = nominal_voltage;
+	msm8625q_acpuclk_pdata.default_turbo_voltage = default_turbo_voltage;
+	return 0;
+}
+
 int __init msm7x2x_misc_init(void)
 {
 	if (machine_is_msm8625_rumi3()) {
@@ -2046,8 +2076,14 @@
 	msm_clock_init(&msm7x27a_clock_init_data);
 	if (cpu_is_msm7x27aa() || cpu_is_msm7x25ab())
 		platform_device_register(&msm7x27aa_device_acpuclk);
-	else if (cpu_is_msm8625()) {
-		if (msm8625_cpu_id() == MSM8625)
+	else if (cpu_is_msm8625q()) {
+			msm_acpuclock_init(1050000, 0);
+			platform_device_register(&msm8625q_device_acpuclk);
+	} else if (cpu_is_msm8625()) {
+		if (machine_is_qrd_skud_prime()) {
+			msm_acpuclock_init(1150000, 1275000);
+			platform_device_register(&msm8625q_device_acpuclk);
+		} else if (msm8625_cpu_id() == MSM8625)
 			platform_device_register(&msm7x27aa_device_acpuclk);
 		else if (msm8625_cpu_id() == MSM8625A)
 			platform_device_register(&msm8625_device_acpuclk);
@@ -2057,11 +2093,11 @@
 		platform_device_register(&msm7x27a_device_acpuclk);
 	}
 
-	if (cpu_is_msm8625() &&
-			(SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 2))
+	if (cpu_is_msm8625() || (cpu_is_msm8625q() &&
+			SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 2))
 		msm_cpr_init();
 
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		pl310_resources[1].start = INT_L2CC_INTR;
 
 	platform_device_register(&pl310_erp_device);
@@ -2083,7 +2119,7 @@
 		   (0x2 << L2X0_AUX_CTRL_WAY_SIZE_SHIFT) | \
 		   (0x1 << L2X0_AUX_CTRL_EVNT_MON_BUS_EN_SHIFT);
 
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		/* Way Size 011(0x3) 64KB */
 		aux_ctrl |= (0x3 << L2X0_AUX_CTRL_WAY_SIZE_SHIFT) | \
 			    (0x1 << L2X0_AUX_CTRL_DATA_PREFETCH_SHIFT) | \
@@ -2099,7 +2135,7 @@
 	}
 
 	l2x0_init(MSM_L2CC_BASE, aux_ctrl, L2X0_AUX_CTRL_MASK);
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		pctrl = readl_relaxed(MSM_L2CC_BASE + L2X0_PREFETCH_CTRL);
 		pr_info("Prfetch Ctrl: 0x%08x\n", pctrl);
 	}
@@ -2136,7 +2172,7 @@
 
 static int msm7627a_init_gpio(void)
 {
-	if (cpu_is_msm8625())
+	if (cpu_is_msm8625() || cpu_is_msm8625q())
 		platform_device_register(&msm8625_device_gpio);
 	else
 		platform_device_register(&msm_device_gpio);
diff --git a/arch/arm/mach-msm/devices-msm8x60.c b/arch/arm/mach-msm/devices-msm8x60.c
index c6513d9..0a080b1 100644
--- a/arch/arm/mach-msm/devices-msm8x60.c
+++ b/arch/arm/mach-msm/devices-msm8x60.c
@@ -100,9 +100,19 @@
 #define MSM_UART9DM_PHYS    (MSM_GSBI9_PHYS + 0x40000)
 #define INT_UART9DM_IRQ     GSBI9_UARTDM_IRQ
 
+static struct resource msm_gpio_resources[] = {
+	{
+		.start	= TLMM_MSM_SUMMARY_IRQ,
+		.end	= TLMM_MSM_SUMMARY_IRQ,
+		.flags	= IORESOURCE_IRQ,
+	},
+};
+
 struct platform_device msm_gpio_device = {
 	.name = "msmgpio",
 	.id = -1,
+	.num_resources	= ARRAY_SIZE(msm_gpio_resources),
+	.resource	= msm_gpio_resources,
 };
 
 static void charm_ap2mdm_kpdpwr_on(void)
diff --git a/arch/arm/mach-msm/devices.h b/arch/arm/mach-msm/devices.h
index c0d73c2..b676518 100644
--- a/arch/arm/mach-msm/devices.h
+++ b/arch/arm/mach-msm/devices.h
@@ -471,4 +471,5 @@
 extern struct platform_device apq_cpudai_mi2s;
 extern struct platform_device apq_cpudai_i2s_rx;
 extern struct platform_device apq_cpudai_i2s_tx;
+extern struct dev_avtimer_data dev_avtimer_pdata;
 
diff --git a/arch/arm/mach-msm/hsic_sysmon.c b/arch/arm/mach-msm/hsic_sysmon.c
index 153e1b4..e088435 100644
--- a/arch/arm/mach-msm/hsic_sysmon.c
+++ b/arch/arm/mach-msm/hsic_sysmon.c
@@ -410,6 +410,7 @@
 static const struct usb_device_id hsic_sysmon_ids[] = {
 	{ USB_DEVICE(0x5c6, 0x9048), .driver_info = 1, },
 	{ USB_DEVICE(0x5c6, 0x904C), .driver_info = 1, },
+	{ USB_DEVICE(0x5c6, 0x9075), .driver_info = 1, },
 	{} /* terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, hsic_sysmon_ids);
diff --git a/arch/arm/mach-msm/idle-v7.S b/arch/arm/mach-msm/idle-v7.S
index ccd0bf7..9a22996 100644
--- a/arch/arm/mach-msm/idle-v7.S
+++ b/arch/arm/mach-msm/idle-v7.S
@@ -96,20 +96,6 @@
 	mrc     p15, 0, r9, c13, c0, 3 /* TPIDRURO */
 	mrc     p15, 0, ip, c13, c0, 1 /* context ID */
 	stmia   r0!, {r1-r9, ip}
-#ifdef CONFIG_MSM_CPU_AVS
-	mrc     p15, 7, r1, c15, c1, 7 /* AVSCSR is the Adaptive Voltage Scaling
-	                                * Control and Status Register */
-	mrc     p15, 7, r2, c15, c0, 6 /* AVSDSCR is the Adaptive Voltage
-	                                * Scaling Delay Synthesizer Control
-					* Register */
-#ifndef CONFIG_ARCH_MSM_KRAIT
-	mrc     p15, 7, r3, c15, c1, 0 /* TSCSR is the Temperature Status and
-	                                * Control Register
-					*/
-#endif
-
-	stmia   r0!, {r1-r3}
-#endif
 
 #ifdef CONFIG_MSM_JTAG
 	bl      msm_jtag_save_state
@@ -240,14 +226,6 @@
 	add	r1, r1, r2
 #endif
 
-#ifdef CONFIG_MSM_CPU_AVS
-	ldmdb   r1!, {r2-r4}
-#ifndef CONFIG_ARCH_MSM_KRAIT
-	mcr     p15, 7, r4, c15, c1, 0 /* TSCSR */
-#endif
-	mcr     p15, 7, r3, c15, c0, 6 /* AVSDSCR */
-	mcr     p15, 7, r2, c15, c1, 7 /* AVSCSR */
-#endif
 	ldmdb   r1!, {r2-r11}
 	mcr     p15, 0, r4, c3, c0, 0 /* dacr */
 	mcr     p15, 0, r3, c2, c0, 0 /* TTBR0 */
diff --git a/arch/arm/mach-msm/idle.h b/arch/arm/mach-msm/idle.h
index ee3209c..5550e96 100644
--- a/arch/arm/mach-msm/idle.h
+++ b/arch/arm/mach-msm/idle.h
@@ -14,13 +14,8 @@
 #ifndef _ARCH_ARM_MACH_MSM_IDLE_H_
 #define _ARCH_ARM_MACH_MSM_IDLE_H_
 
-#ifdef CONFIG_MSM_CPU_AVS
-/* 11 general purpose registers (r4-r14), 10 cp15 registers, 3 AVS registers */
-#define CPU_SAVED_STATE_SIZE (4 * 11 + 4 * 10 + 4 * 3)
-#else
 /* 11 general purpose registers (r4-r14), 10 cp15 registers */
 #define CPU_SAVED_STATE_SIZE (4 * 11 + 4 * 10)
-#endif
 
 #define ON	1
 #define OFF	0
diff --git a/arch/arm/mach-msm/include/mach/iommu.h b/arch/arm/mach-msm/include/mach/iommu.h
index ea3fb64..975b12c 100644
--- a/arch/arm/mach-msm/include/mach/iommu.h
+++ b/arch/arm/mach-msm/include/mach/iommu.h
@@ -83,6 +83,7 @@
 /**
  * struct msm_iommu_drvdata - A single IOMMU hardware instance
  * @base:	IOMMU config port base address (VA)
+ * @glb_base:	IOMMU config port base address for global register space (VA)
  * @ncb		The number of contexts on this IOMMU
  * @irq:	Interrupt number
  * @clk:	The bus clock for this IOMMU hardware instance
@@ -99,6 +100,7 @@
  */
 struct msm_iommu_drvdata {
 	void __iomem *base;
+	void __iomem *glb_base;
 	int ncb;
 	int ttbr_split;
 	struct clk *clk;
@@ -123,8 +125,8 @@
  *			attached to them
  * @attached_domain	Domain currently attached to this context (if any)
  * @name		Human-readable name of this context device
- * @sids		List of Stream IDs mapped to this context (v2 only)
- * @nsid		Number of Stream IDs mapped to this context (v2 only)
+ * @sids		List of Stream IDs mapped to this context
+ * @nsid		Number of Stream IDs mapped to this context
  *
  * A msm_iommu_ctx_drvdata holds the driver data for a single context bank
  * within each IOMMU hardware instance
@@ -231,6 +233,12 @@
 		of_node_put(node);
 		return 0;
 	}
+
+	node = of_find_compatible_node(NULL, NULL, "qcom,msm-smmu-v1");
+	if (node) {
+		of_node_put(node);
+		return 1;
+	}
 #endif
 	if (cpu_is_msm8960() &&
 	    SOCINFO_VERSION_MAJOR(socinfo_get_version()) < 2)
diff --git a/arch/arm/mach-msm/include/mach/iommu_hw-8xxx.h b/arch/arm/mach-msm/include/mach/iommu_hw-8xxx.h
index af82fd9..31b2b4f 100644
--- a/arch/arm/mach-msm/include/mach/iommu_hw-8xxx.h
+++ b/arch/arm/mach-msm/include/mach/iommu_hw-8xxx.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2010-2011, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2010-2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -811,24 +811,24 @@
 
 
 /* Global Registers */
-#define M2VCBR_N	(0xFF000)
-#define CBACR_N		(0xFF800)
-#define TLBRSW		(0xFFE00)
-#define TLBTR0		(0xFFE80)
-#define TLBTR1		(0xFFE84)
-#define TLBTR2		(0xFFE88)
-#define TESTBUSCR	(0xFFE8C)
-#define GLOBAL_TLBIALL	(0xFFF00)
-#define TLBIVMID	(0xFFF04)
-#define CR		(0xFFF80)
-#define EAR		(0xFFF84)
-#define ESR		(0xFFF88)
-#define ESRRESTORE	(0xFFF8C)
-#define ESYNR0		(0xFFF90)
-#define ESYNR1		(0xFFF94)
-#define REV		(0xFFFF4)
-#define IDR		(0xFFFF8)
-#define RPU_ACR		(0xFFFFC)
+#define M2VCBR_N	(0x000)
+#define CBACR_N		(0x800)
+#define TLBRSW		(0xE00)
+#define TLBTR0		(0xE80)
+#define TLBTR1		(0xE84)
+#define TLBTR2		(0xE88)
+#define TESTBUSCR	(0xE8C)
+#define GLOBAL_TLBIALL	(0xF00)
+#define TLBIVMID	(0xF04)
+#define CR		(0xF80)
+#define EAR		(0xF84)
+#define ESR		(0xF88)
+#define ESRRESTORE	(0xF8C)
+#define ESYNR0		(0xF90)
+#define ESYNR1		(0xF94)
+#define REV		(0xFF4)
+#define IDR		(0xFF8)
+#define RPU_ACR		(0xFFC)
 
 
 /* Context Bank Registers */
diff --git a/arch/arm/mach-msm/include/mach/ipa.h b/arch/arm/mach-msm/include/mach/ipa.h
index dae6d3b..c37b518 100644
--- a/arch/arm/mach-msm/include/mach/ipa.h
+++ b/arch/arm/mach-msm/include/mach/ipa.h
@@ -457,7 +457,7 @@
 
 int ipa_teardown_sys_pipe(u32 clnt_hdl);
 
-#else
+#else /* CONFIG_IPA */
 
 /*
  * Connect / Disconnect
diff --git a/arch/arm/mach-msm/include/mach/irqs-8974.h b/arch/arm/mach-msm/include/mach/irqs-8974.h
index f18b3df..d11c35c 100644
--- a/arch/arm/mach-msm/include/mach/irqs-8974.h
+++ b/arch/arm/mach-msm/include/mach/irqs-8974.h
@@ -20,7 +20,6 @@
 
 #define APCC_QGICL2PERFMONIRPTREQ	(GIC_SPI_START + 1)
 #define SC_SICL2PERFMONIRPTREQ		APCC_QGICL2PERFMONIRPTREQ
-#define TLMM_MSM_SUMMARY_IRQ		(GIC_SPI_START + 208)
 
 #endif
 
diff --git a/arch/arm/mach-msm/include/mach/irqs-9625.h b/arch/arm/mach-msm/include/mach/irqs-9625.h
index 15c7010..9ba77af 100644
--- a/arch/arm/mach-msm/include/mach/irqs-9625.h
+++ b/arch/arm/mach-msm/include/mach/irqs-9625.h
@@ -19,7 +19,6 @@
 
 #define APCC_QGICL2PERFMONIRPTREQ	(GIC_SPI_START + 1)
 #define SC_SICL2PERFMONIRPTREQ		APCC_QGICL2PERFMONIRPTREQ
-#define TLMM_MSM_SUMMARY_IRQ		(GIC_SPI_START + 208)
 
 #define NR_MSM_IRQS 288
 #define NR_GPIO_IRQS 76
diff --git a/arch/arm/mach-msm/include/mach/irqs.h b/arch/arm/mach-msm/include/mach/irqs.h
index f5822fc..8c6b959 100644
--- a/arch/arm/mach-msm/include/mach/irqs.h
+++ b/arch/arm/mach-msm/include/mach/irqs.h
@@ -46,7 +46,6 @@
 
 #elif defined(CONFIG_ARCH_MSM8910) || defined(CONFIG_ARCH_MSM8226)
 
-#define TLMM_MSM_SUMMARY_IRQ		(GIC_SPI_START + 208)
 #define NR_MSM_IRQS 256
 #define NR_GPIO_IRQS 117
 #define NR_QPNP_IRQS 32768
diff --git a/arch/arm/mach-msm/include/mach/msm_adsp.h b/arch/arm/mach-msm/include/mach/msm_adsp.h
index e40c07d..ea08f0c 100644
--- a/arch/arm/mach-msm/include/mach/msm_adsp.h
+++ b/arch/arm/mach-msm/include/mach/msm_adsp.h
@@ -39,6 +39,7 @@
 void msm_adsp_put(struct msm_adsp_module *module);
 int msm_adsp_enable(struct msm_adsp_module *module);
 int msm_adsp_disable(struct msm_adsp_module *module);
+int msm_adsp_dump(struct msm_adsp_module *module);
 int adsp_set_clkrate(struct msm_adsp_module *module, unsigned long clk_rate);
 int msm_adsp_disable_event_rsp(struct msm_adsp_module *module);
 int32_t get_adsp_resource(unsigned short client_idx,
diff --git a/arch/arm/mach-msm/include/mach/msm_dcvs_scm.h b/arch/arm/mach-msm/include/mach/msm_dcvs_scm.h
index 597fdc0..7eefd54 100644
--- a/arch/arm/mach-msm/include/mach/msm_dcvs_scm.h
+++ b/arch/arm/mach-msm/include/mach/msm_dcvs_scm.h
@@ -48,7 +48,7 @@
 	uint32_t slack_time_min_us;
 	uint32_t slack_time_max_us;
 	uint32_t slack_weight_thresh_pct;
-	uint32_t ss_iobusy_conv;
+	uint32_t ss_no_corr_below_freq;
 	uint32_t ss_win_size_min_us;
 	uint32_t ss_win_size_max_us;
 	uint32_t ss_util_pct;
diff --git a/arch/arm/mach-msm/include/mach/msm_hdmi_audio_codec.h b/arch/arm/mach-msm/include/mach/msm_hdmi_audio_codec.h
new file mode 100644
index 0000000..95f33d5
--- /dev/null
+++ b/arch/arm/mach-msm/include/mach/msm_hdmi_audio_codec.h
@@ -0,0 +1,37 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MSM_HDMI_AUDIO_CODEC_H__
+#define __MSM_HDMI_AUDIO_CODEC_H__
+
+#include <linux/device.h>
+#include <linux/platform_device.h>
+
+struct msm_hdmi_audio_edid_blk {
+	u8 *audio_data_blk;
+	unsigned int audio_data_blk_size; /* in bytes */
+	u8 *spk_alloc_data_blk;
+	unsigned int spk_alloc_data_blk_size; /* in bytes */
+};
+
+struct msm_hdmi_audio_codec_ops {
+	int (*audio_info_setup)(struct platform_device *pdev,
+		u32 num_of_channels, u32 channel_allocation, u32 level_shift,
+		bool down_mix);
+	int (*get_audio_edid_blk) (struct platform_device *pdev,
+		struct msm_hdmi_audio_edid_blk *blk);
+};
+
+int msm_hdmi_register_audio_codec(struct platform_device *pdev,
+	struct msm_hdmi_audio_codec_ops *ops);
+
+#endif /* __MSM_HDMI_AUDIO_CODEC_H__ */
diff --git a/arch/arm/mach-msm/include/mach/msm_iomap-8910.h b/arch/arm/mach-msm/include/mach/msm_iomap-8910.h
index 08f21b6..64990da 100644
--- a/arch/arm/mach-msm/include/mach/msm_iomap-8910.h
+++ b/arch/arm/mach-msm/include/mach/msm_iomap-8910.h
@@ -22,7 +22,7 @@
  *
  */
 
-#define MSM8910_MSM_SHARED_RAM_PHYS	0x0FA00000
+#define MSM8910_MSM_SHARED_RAM_PHYS	0x0D600000
 
 #define MSM8910_APCS_GCC_PHYS	0xF9011000
 #define MSM8910_APCS_GCC_SIZE	SZ_4K
diff --git a/arch/arm/mach-msm/include/mach/msm_iomap-8x60.h b/arch/arm/mach-msm/include/mach/msm_iomap-8x60.h
index 17156b1..fe928b9 100644
--- a/arch/arm/mach-msm/include/mach/msm_iomap-8x60.h
+++ b/arch/arm/mach-msm/include/mach/msm_iomap-8x60.h
@@ -127,6 +127,7 @@
 #define MSM_HDMI_SIZE		SZ_4K
 
 /* Needed to keep the unified iomap happy */
+#define MSM_APCS_GCC_BASE       IOMEM(0xFA006000)       /*  4K  */
 #define MSM_MPM2_PSHOLD_BASE	MSM_TLMM_BASE
 
 #ifdef CONFIG_DEBUG_MSM8660_UART
diff --git a/arch/arm/mach-msm/include/mach/rpc_server_handset.h b/arch/arm/mach-msm/include/mach/rpc_server_handset.h
index e1dc841..0856a4a 100644
--- a/arch/arm/mach-msm/include/mach/rpc_server_handset.h
+++ b/arch/arm/mach-msm/include/mach/rpc_server_handset.h
@@ -17,6 +17,7 @@
 struct msm_handset_platform_data {
 	const char *hs_name;
 	uint32_t pwr_key_delay_ms; /* default 500ms */
+	bool ignore_end_key;
 };
 
 void report_headset_status(bool connected);
diff --git a/arch/arm/mach-msm/include/mach/sensors_adsp.h b/arch/arm/mach-msm/include/mach/sensors_adsp.h
new file mode 100644
index 0000000..3c65e37
--- /dev/null
+++ b/arch/arm/mach-msm/include/mach/sensors_adsp.h
@@ -0,0 +1,111 @@
+/* Copyright (c) 2012,  The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef SENSORS_ADSP_H
+#define SENSORS_ADSP_H
+
+#include <linux/types.h>
+
+/** Maximum number of segments that may be mapped from DDR to OCMEM  */
+#define SNS_OCMEM_MAX_NUM_SEG_V01 16
+
+/**  Maximum size of the ocmem_vectors structure  */
+#define SNS_OCMEM_MAX_VECTORS_SIZE_V01 512
+
+/* Sensor OCMEM message id  */
+
+#define SNS_OCMEM_CANCEL_REQ_V01 0x0000
+#define SNS_OCMEM_CANCEL_RESP_V01 0x0000
+#define SNS_OCMEM_VERSION_REQ_V01 0x0001
+#define SNS_OCMEM_VERSION_RESP_V01 0x0001
+#define SNS_OCMEM_PHYS_ADDR_REQ_V01 0x0002
+#define SNS_OCMEM_PHYS_ADDR_RESP_V01 0x0002
+#define SNS_OCMEM_HAS_CLIENT_IND_V01 0x0002
+#define SNS_OCMEM_BW_VOTE_REQ_V01 0x0003
+#define SNS_OCMEM_BW_VOTE_RESP_V01 0x0003
+#define SNS_OCMEM_BW_VOTE_IND_V01 0x0003
+
+enum {
+	SNS_OCMEM_MODULE_KERNEL = 0,
+	SNS_OCMEM_MODULE_ADSP
+};
+
+/**
+ * Defines the types of response messages
+ */
+enum {
+	SNS_OCMEM_MSG_TYPE_REQ = 0,  /* Request */
+	SNS_OCMEM_MSG_TYPE_RESP,     /* Response to a request */
+	SNS_OCMEM_MSG_TYPE_IND       /* Asynchronous indication */
+};
+
+/**
+ * The message header. Used in both incoming and outgoing messages
+ */
+struct sns_ocmem_hdr_s {
+	int32_t  msg_id ;	/* Message ID, as defined in the IDL */
+	uint16_t msg_size;	/* Size of message, in bytes */
+	uint8_t  dst_module;	/* Destination module */
+	uint8_t  src_module;	/* Source module */
+	uint8_t  msg_type;	/* The message type */
+} __packed;
+
+struct sns_ocmem_common_resp_s_v01 {
+	/*  This shall be the first element of every response message  */
+	uint8_t sns_result_t;
+	/**<   0 == SUCCESS; 1 == FAILURE
+	A result of FAILURE indicates that that any data contained in the
+	response should not be used other than sns_err_t, to determine the
+	type of error */
+	uint8_t sns_err_t;
+	/**<   See sns_ocmem_error_e in ocmem_sensors.h */
+};
+
+/* This structure represents a single memory region that must be
+mapped from DDR to OCMEM */
+struct sns_mem_segment_s_v01 {
+
+	uint64_t start_address; /* Physical start address of segment */
+	uint32_t size; /* Size (in bytes) of this segment */
+	uint16_t type; /*  1 == Read only; 2 == Read/Write Data */
+} __packed;
+
+struct sns_ocmem_phys_addr_resp_msg_v01 {
+	struct sns_ocmem_common_resp_s_v01 resp; /* response */
+	uint32_t segments_len; /* number of elements in segments */
+	/* Segments mapped from DDR to OCMEM */
+	struct sns_mem_segment_s_v01 segments[SNS_OCMEM_MAX_NUM_SEG_V01];
+	uint8_t segments_valid; /* true if segments is being passed */
+} __packed ;
+
+struct sns_ocmem_has_client_ind_msg_v01 {
+	uint16_t num_clients; /* Number of active clients on the ADSP */
+} __packed;
+
+struct sns_ocmem_bw_vote_req_msg_v01 {
+	uint8_t is_map;		/* True if mapping; false if unmapping */
+	uint8_t vectors_valid;  /* True if vectors is being passed */
+	uint32_t vectors_len;	/* Number of elements in vectors */
+	uint8_t vectors[SNS_OCMEM_MAX_VECTORS_SIZE_V01]; /* vectors */
+} __packed;
+
+struct sns_ocmem_bw_vote_resp_msg_v01 {
+	struct sns_ocmem_common_resp_s_v01 resp;
+};
+
+struct sns_ocmem_bw_vote_ind_msg_v01 {
+	/* If the ADSP just voted for, or took away its vote for
+	OCMEM bandwidth */
+	uint8_t is_vote_on;
+} __packed;
+
+#endif /* SENSORS_ADSP_H */
diff --git a/arch/arm/mach-msm/include/mach/socinfo.h b/arch/arm/mach-msm/include/mach/socinfo.h
index 0499a7a..c0624bb 100644
--- a/arch/arm/mach-msm/include/mach/socinfo.h
+++ b/arch/arm/mach-msm/include/mach/socinfo.h
@@ -54,12 +54,16 @@
 	of_machine_is_compatible("qcom,msm8226")
 #define machine_is_msm8226_sim()		\
 	of_machine_is_compatible("qcom,msm8226-sim")
+#define machine_is_msm8226_rumi()		\
+	of_machine_is_compatible("qcom,msm8226-rumi")
 #define early_machine_is_msm8910()	\
 	of_flat_dt_is_compatible(of_get_flat_dt_root(), "qcom,msm8910")
 #define machine_is_msm8910()		\
 	of_machine_is_compatible("qcom,msm8910")
 #define machine_is_msm8910_sim()		\
 	of_machine_is_compatible("qcom,msm8910-sim")
+#define machine_is_msm8910_rumi()		\
+	of_machine_is_compatible("qcom,msm8910-rumi")
 #else
 #define early_machine_is_msm8974()	0
 #define machine_is_msm8974()		0
@@ -72,9 +76,11 @@
 #define early_machine_is_msm8226()	0
 #define machine_is_msm8226()		0
 #define machine_is_msm8226_sim()	0
+#define machine_is_msm8226_rumi()	0
 #define early_machine_is_msm8910()	0
 #define machine_is_msm8910()		0
 #define machine_is_msm8910_sim()	0
+#define machine_is_msm8910_rumi()	0
 
 #endif
 
diff --git a/arch/arm/mach-msm/include/mach/subsystem_restart.h b/arch/arm/mach-msm/include/mach/subsystem_restart.h
index 64190d2..17b11e1 100644
--- a/arch/arm/mach-msm/include/mach/subsystem_restart.h
+++ b/arch/arm/mach-msm/include/mach/subsystem_restart.h
@@ -63,6 +63,7 @@
 extern int get_restart_level(void);
 extern int subsystem_restart_dev(struct subsys_device *dev);
 extern int subsystem_restart(const char *name);
+extern int subsystem_crashed(const char *name);
 
 extern void *subsystem_get(const char *name);
 extern void subsystem_put(void *subsystem);
@@ -89,6 +90,11 @@
 	return 0;
 }
 
+static inline int subsystem_crashed(const char *name)
+{
+	return 0;
+}
+
 static inline void *subsystem_get(const char *name)
 {
 	return NULL;
diff --git a/arch/arm/mach-msm/lpm_resources.c b/arch/arm/mach-msm/lpm_resources.c
index c21ea33..5d7fc94 100644
--- a/arch/arm/mach-msm/lpm_resources.c
+++ b/arch/arm/mach-msm/lpm_resources.c
@@ -47,8 +47,6 @@
 static bool msm_lpm_get_rpm_notif = true;
 
 /*Macros*/
-#define VDD_DIG_ACTIVE		(5)
-#define VDD_MEM_ACTIVE		(1050000)
 #define MAX_RS_NAME		(16)
 #define MAX_RS_SIZE		(4)
 #define IS_RPM_CTL(rs) \
@@ -133,10 +131,6 @@
 	.flush = msm_lpm_flush_l2,
 	.notify = NULL,
 	.valid = false,
-	.rs_data = {
-		.value = MSM_LPM_L2_CACHE_ACTIVE,
-		.default_value = MSM_LPM_L2_CACHE_ACTIVE,
-	},
 	.ko_attr = RPMRS_ATTR(l2),
 };
 
@@ -147,10 +141,6 @@
 	.flush = msm_lpm_flush_vdd_dig,
 	.notify = msm_lpm_notify_vdd_dig,
 	.valid = false,
-	.rs_data = {
-		.value = VDD_DIG_ACTIVE,
-		.default_value = VDD_DIG_ACTIVE,
-	},
 	.ko_attr = RPMRS_ATTR(vdd_dig),
 };
 
@@ -161,10 +151,6 @@
 	.flush = msm_lpm_flush_vdd_mem,
 	.notify = msm_lpm_notify_vdd_mem,
 	.valid = false,
-	.rs_data = {
-		.value = VDD_MEM_ACTIVE,
-		.default_value = VDD_MEM_ACTIVE,
-	},
 	.ko_attr = RPMRS_ATTR(vdd_mem),
 };
 
@@ -175,10 +161,6 @@
 	.flush = msm_lpm_flush_pxo,
 	.notify = msm_lpm_notify_pxo,
 	.valid = false,
-	.rs_data = {
-		.value = MSM_LPM_PXO_ON,
-		.default_value = MSM_LPM_PXO_ON,
-	},
 	.ko_attr = RPMRS_ATTR(pxo),
 };
 
@@ -421,13 +403,13 @@
 	trace_lpm_resources(rs->sleep_value, rs->name);
 }
 
-static void msm_lpm_flush_l2(int notify_rpm)
+static void msm_lpm_set_l2_mode(int sleep_mode, int notify_rpm)
 {
-	struct msm_lpm_resource *rs = &msm_lpm_l2;
-	int lpm;
-	int rc;
+	int lpm, rc;
 
-	switch (rs->sleep_value) {
+	msm_pm_set_l2_flush_flag(0);
+
+	switch (sleep_mode) {
 	case MSM_LPM_L2_CACHE_HSFS_OPEN:
 		lpm = MSM_SPM_L2_MODE_POWER_COLLAPSE;
 		msm_pm_set_l2_flush_flag(1);
@@ -455,6 +437,13 @@
 				__func__, lpm);
 }
 
+static void msm_lpm_flush_l2(int notify_rpm)
+{
+	struct msm_lpm_resource *rs = &msm_lpm_l2;
+
+	msm_lpm_set_l2_mode(rs->sleep_value, notify_rpm);
+}
+
 /* RPM CTL */
 static void msm_lpm_flush_rpm_ctl(int notify_rpm)
 {
@@ -679,8 +668,7 @@
 	}
 	msm_lpm_get_rpm_notif = true;
 
-	if (msm_lpm_use_mpm(limits))
-		msm_mpm_enter_sleep(sclk_count, from_idle);
+	msm_mpm_enter_sleep(sclk_count, from_idle);
 
 	return ret;
 }
@@ -688,11 +676,11 @@
 void msm_lpmrs_exit_sleep(struct msm_rpmrs_limits *limits,
 		bool from_idle, bool notify_rpm, bool collapsed)
 {
-	/* MPM exit sleep
 	if (msm_lpm_use_mpm(limits))
-		msm_mpm_exit_sleep(from_idle);*/
+		msm_mpm_exit_sleep(from_idle);
 
-	msm_spm_l2_set_low_power_mode(MSM_SPM_MODE_DISABLED, notify_rpm);
+	if (msm_lpm_l2.valid)
+		msm_lpm_set_l2_mode(msm_lpm_l2.rs_data.default_value, false);
 }
 
 static int msm_lpm_cpu_callback(struct notifier_block *cpu_nb,
@@ -702,12 +690,12 @@
 	switch (action) {
 	case CPU_UP_PREPARE:
 	case CPU_UP_PREPARE_FROZEN:
-		rs->rs_data.value = MSM_LPM_L2_CACHE_ACTIVE;
+		rs->rs_data.value = rs->rs_data.default_value;
 		break;
 	case CPU_ONLINE_FROZEN:
 	case CPU_ONLINE:
 		if (num_online_cpus() > 1)
-			rs->rs_data.value = MSM_LPM_L2_CACHE_ACTIVE;
+			rs->rs_data.value = rs->rs_data.default_value;
 		break;
 	case CPU_DEAD_FROZEN:
 	case CPU_DEAD:
@@ -825,6 +813,16 @@
 			continue;
 		}
 
+		key = "qcom,init-value";
+		ret = of_property_read_u32(node, key,
+				&rs->rs_data.default_value);
+		if (ret) {
+			pr_err("%s():Failed to read %s\n", __func__, key);
+			goto fail;
+		}
+
+		rs->rs_data.value = rs->rs_data.default_value;
+
 		key = "qcom,resource-type";
 		ret = of_property_read_u32(node, key, &resource_type);
 		if (ret) {
diff --git a/arch/arm/mach-msm/mdm2.c b/arch/arm/mach-msm/mdm2.c
index 77eeb53..0e3e8e0 100644
--- a/arch/arm/mach-msm/mdm2.c
+++ b/arch/arm/mach-msm/mdm2.c
@@ -118,11 +118,13 @@
 		}
 		msleep(100);
 	}
+
+	/* Assert the soft reset line whether mdm2ap_status went low or not */
+	gpio_direction_output(mdm_drv->ap2mdm_soft_reset_gpio,
+					soft_reset_direction);
 	if (i == 0) {
 		pr_err("%s: MDM2AP_STATUS never went low. Doing a hard reset\n",
 			   __func__);
-		gpio_direction_output(mdm_drv->ap2mdm_soft_reset_gpio,
-					soft_reset_direction);
 		/*
 		* Currently, there is a debounce timer on the charm PMIC. It is
 		* necessary to hold the PMIC RESET low for ~3.5 seconds
diff --git a/arch/arm/mach-msm/msm-buspm-dev.c b/arch/arm/mach-msm/msm-buspm-dev.c
index a818eed..ec0f1bd 100644
--- a/arch/arm/mach-msm/msm-buspm-dev.c
+++ b/arch/arm/mach-msm/msm-buspm-dev.c
@@ -22,10 +22,17 @@
 #include <linux/uaccess.h>
 #include <linux/miscdevice.h>
 #include <linux/memory_alloc.h>
+#include <mach/rpm-smd.h>
 #include "msm-buspm-dev.h"
 
 #define MSM_BUSPM_DRV_NAME "msm-buspm-dev"
 
+enum msm_buspm_spdm_res {
+	SPDM_RES_ID = 0,
+	SPDM_RES_TYPE = 0x63707362,
+	SPDM_KEY = 0x00006e65,
+	SPDM_SIZE = 4,
+};
 /*
  * Allocate kernel buffer.
  * Currently limited to one buffer per file descriptor.  If alloc() is
@@ -113,6 +120,61 @@
 	return 0;
 }
 
+static int msm_bus_rpm_req(u32 rsc_type, u32 key, u32 hwid,
+	int ctx, u32 val)
+{
+	struct msm_rpm_request *rpm_req;
+	int ret, msg_id;
+
+	rpm_req = msm_rpm_create_request(ctx, rsc_type, SPDM_RES_ID, 1);
+	if (rpm_req == NULL) {
+		pr_err("RPM: Couldn't create RPM Request\n");
+		return -ENXIO;
+	}
+
+	ret = msm_rpm_add_kvp_data(rpm_req, key, (const uint8_t *)&val,
+		(int)(sizeof(uint32_t)));
+	if (ret) {
+		pr_err("RPM: Add KVP failed for RPM Req:%u\n",
+			rsc_type);
+		goto err;
+	}
+
+	pr_debug("Added Key: %d, Val: %u, size: %d\n", key,
+		(uint32_t)val, sizeof(uint32_t));
+	msg_id = msm_rpm_send_request(rpm_req);
+	if (!msg_id) {
+		pr_err("RPM: No message ID for req\n");
+		ret = -ENXIO;
+		goto err;
+	}
+
+	ret = msm_rpm_wait_for_ack(msg_id);
+	if (ret) {
+		pr_err("RPM: Ack failed\n");
+		goto err;
+	}
+
+err:
+	msm_rpm_free_request(rpm_req);
+	return ret;
+}
+
+static int msm_buspm_ioc_cmds(uint32_t arg)
+{
+	switch (arg) {
+	case MSM_BUSPM_SPDM_CLK_DIS:
+	case MSM_BUSPM_SPDM_CLK_EN:
+		return msm_bus_rpm_req(SPDM_RES_TYPE, SPDM_KEY, 0,
+				MSM_RPM_CTX_ACTIVE_SET, arg);
+	default:
+		pr_warn("Unsupported ioctl command: %d\n", arg);
+		return -EINVAL;
+	}
+}
+
+
+
 static long
 msm_buspm_dev_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
@@ -124,6 +186,11 @@
 	unsigned int buflen = msm_buspm_dev_get_buflen(filp);
 	unsigned char *dbgbuf = buf;
 
+	if (_IOC_TYPE(cmd) != MSM_BUSPM_IOC_MAGIC) {
+		pr_err("Wrong IOC_MAGIC.Exiting\n");
+		return -ENOTTY;
+	}
+
 	switch (cmd) {
 	case MSM_BUSPM_IOC_FREE:
 		pr_debug("cmd = 0x%x (FREE)\n", cmd);
@@ -193,6 +260,11 @@
 		}
 		break;
 
+	case MSM_BUSPM_IOC_CMD:
+		pr_debug("IOCTL command: cmd: %d arg: %lu\n", cmd, arg);
+		retval = msm_buspm_ioc_cmds(arg);
+		break;
+
 	default:
 		pr_debug("Unknown command 0x%x\n", cmd);
 		retval = -EINVAL;
diff --git a/arch/arm/mach-msm/msm-buspm-dev.h b/arch/arm/mach-msm/msm-buspm-dev.h
index 5839087..854626d 100644
--- a/arch/arm/mach-msm/msm-buspm-dev.h
+++ b/arch/arm/mach-msm/msm-buspm-dev.h
@@ -31,6 +31,11 @@
 	int size;
 };
 
+enum msm_buspm_ioc_cmds {
+	MSM_BUSPM_SPDM_CLK_DIS = 0,
+	MSM_BUSPM_SPDM_CLK_EN,
+};
+
 #define MSM_BUSPM_IOC_MAGIC	'p'
 
 #define MSM_BUSPM_IOC_FREE	\
@@ -47,4 +52,7 @@
 
 #define MSM_BUSPM_IOC_RD_PHYS_ADDR	\
 	_IOR(MSM_BUSPM_IOC_MAGIC, 4, unsigned long)
+
+#define MSM_BUSPM_IOC_CMD	\
+	_IOR(MSM_BUSPM_IOC_MAGIC, 5, uint32_t)
 #endif
diff --git a/arch/arm/mach-msm/msm7k_fiq.c b/arch/arm/mach-msm/msm7k_fiq.c
index 421b4f9..d644121 100644
--- a/arch/arm/mach-msm/msm7k_fiq.c
+++ b/arch/arm/mach-msm/msm7k_fiq.c
@@ -75,7 +75,7 @@
 
 static int __init init7k_fiq(void)
 {
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		return 0;
 
 	if (msm_setup_fiq_handler())
diff --git a/arch/arm/mach-msm/msm_bus/msm_bus_arb.c b/arch/arm/mach-msm/msm_bus/msm_bus_arb.c
index 65539c6..e61eb6d 100644
--- a/arch/arm/mach-msm/msm_bus/msm_bus_arb.c
+++ b/arch/arm/mach-msm/msm_bus/msm_bus_arb.c
@@ -614,6 +614,15 @@
 			MSM_BUS_DBG("ab: %llu ib: %llu\n", curr_bw, curr_clk);
 		}
 
+		if (index == 0) {
+			/* This check protects the bus driver from clients
+			 * that can leave non-zero requests after
+			 * unregistering.
+			 * */
+			req_clk = 0;
+			req_bw = 0;
+		}
+
 		if (!pdata->active_only) {
 			ret = update_path(src, pnode, req_clk, req_bw,
 				curr_clk, curr_bw, 0, pdata->active_only);
diff --git a/arch/arm/mach-msm/msm_cpr.c b/arch/arm/mach-msm/msm_cpr.c
index c7a8b98..dd6ffab 100644
--- a/arch/arm/mach-msm/msm_cpr.c
+++ b/arch/arm/mach-msm/msm_cpr.c
@@ -91,7 +91,7 @@
 	struct regulator *vreg_cx;
 	const struct msm_cpr_config *config;
 	struct notifier_block freq_transition;
-	struct msm_cpr_vp_data *vp;
+	uint32_t step_size;
 };
 
 /* Need to maintain state data for suspend and resume APIs */
@@ -219,7 +219,7 @@
 	 *
 	 */
 	level_uV = chip_data->turbo_Vmax -
-		(chip_data->tgt_volt_offset * cpr->vp->step_size);
+		(chip_data->tgt_volt_offset * cpr->step_size);
 	msm_cpr_debug(MSM_CPR_DEBUG_CONFIG,
 		"tgt_volt_uV = %d\n", level_uV);
 
@@ -260,7 +260,7 @@
 	quot1 = (cpr_read_reg(cpr, RBCPR_DEBUG1) & QUOT_SLOW_M) >> 12;
 
 	/* Take second CPR measurement at a lower voltage to get QUOT2 */
-	level_uV -= 4 * cpr->vp->step_size;
+	level_uV -= 4 * cpr->step_size;
 	msm_cpr_debug(MSM_CPR_DEBUG_CONFIG,
 		"tgt_volt_uV = %d\n", level_uV);
 
@@ -493,7 +493,7 @@
 		error_step += 1;
 
 		/* Calculte new PMIC voltage */
-		new_volt = curr_volt + (error_step * cpr->vp->step_size);
+		new_volt = curr_volt + (error_step * cpr->step_size);
 		msm_cpr_debug(MSM_CPR_DEBUG_STEPS,
 			"UP_INT: new_volt: %d, error_step=%d\n",
 					new_volt, error_step);
@@ -531,7 +531,7 @@
 			error_step = 2;
 
 		/* Calculte new PMIC voltage */
-		new_volt = curr_volt - (error_step * cpr->vp->step_size);
+		new_volt = curr_volt - (error_step * cpr->step_size);
 		msm_cpr_debug(MSM_CPR_DEBUG_STEPS,
 			"DOWN_INT: new_volt: %d, error_step=%d\n",
 			new_volt, error_step);
@@ -953,7 +953,7 @@
 
 	cpr->base = base;
 
-	cpr->vp = pdata->vp_data;
+	cpr->step_size = pdata->step_size;
 
 	spin_lock_init(&cpr->cpr_lock);
 
diff --git a/arch/arm/mach-msm/msm_cpr.h b/arch/arm/mach-msm/msm_cpr.h
index 3d10478..d9c8e9b 100644
--- a/arch/arm/mach-msm/msm_cpr.h
+++ b/arch/arm/mach-msm/msm_cpr.h
@@ -122,20 +122,6 @@
 };
 
 /**
- * struct msm_vp_data - structure for VP configuration
- * @min_volt: minimum microvolt level for VP
- * @max_volt: maximum microvolt level for VP
- * @default_volt: default microvolt for VP
- * @step_size: step size of voltage in microvolt
- */
-struct msm_cpr_vp_data {
-	int min_volt;
-	int max_volt;
-	int default_volt;
-	int step_size;
-};
-
-/**
  * struct msm_cpr_osc -  Data for CPR ring oscillator
  * @gcnt: gate count value for the oscillator
  * @quot: target value for ring oscillator
@@ -189,7 +175,7 @@
 	uint32_t max_freq;
 	uint32_t max_quot;
 	bool disable_cpr;
-	struct msm_cpr_vp_data *vp_data;
+	uint32_t step_size;
 	uint32_t (*get_quot)(uint32_t max_quot, uint32_t max_freq,
 				uint32_t new_freq);
 	void (*clk_enable)(void);
diff --git a/arch/arm/mach-msm/msm_cpu_pwrctl.c b/arch/arm/mach-msm/msm_cpu_pwrctl.c
new file mode 100644
index 0000000..89b8a09
--- /dev/null
+++ b/arch/arm/mach-msm/msm_cpu_pwrctl.c
@@ -0,0 +1,43 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/errno.h>
+
+static const phys_addr_t primary_cpu_pwrctl_phys = 0x2088004;
+
+static int __init msm_cpu_pwrctl_init(void)
+{
+	void *pwrctl_ptr;
+	unsigned int value;
+	int rc = 0;
+
+	pwrctl_ptr = ioremap_nocache(primary_cpu_pwrctl_phys, SZ_4K);
+	if (unlikely(!pwrctl_ptr)) {
+		pr_err("Failed to remap APCS_CPU_PWR_CTL register for CPU0\n");
+		rc = -EINVAL;
+		goto done;
+	}
+
+	value = readl_relaxed(pwrctl_ptr);
+	value |= 0x100;
+	writel_relaxed(value, pwrctl_ptr);
+	mb();
+	iounmap(pwrctl_ptr);
+
+done:
+	return rc;
+}
+
+early_initcall(msm_cpu_pwrctl_init);
diff --git a/arch/arm/mach-msm/msm_dcvs.c b/arch/arm/mach-msm/msm_dcvs.c
index 3b9656e..9e0be63 100644
--- a/arch/arm/mach-msm/msm_dcvs.c
+++ b/arch/arm/mach-msm/msm_dcvs.c
@@ -49,7 +49,7 @@
 	struct kobj_attribute slack_time_min_us;
 	struct kobj_attribute slack_time_max_us;
 	struct kobj_attribute slack_weight_thresh_pct;
-	struct kobj_attribute ss_iobusy_conv;
+	struct kobj_attribute ss_no_corr_below_freq;
 	struct kobj_attribute ss_win_size_min_us;
 	struct kobj_attribute ss_win_size_max_us;
 	struct kobj_attribute ss_util_pct;
@@ -151,6 +151,7 @@
 static unsigned num_cpu_freqs;
 static struct msm_dcvs_platform_data *dcvs_pdata;
 
+static DEFINE_MUTEX(param_update_mutex);
 static DEFINE_MUTEX(gpu_floor_mutex);
 
 static void force_stop_slack_timer(struct dcvs_core *core)
@@ -309,6 +310,20 @@
 	mutex_unlock(&gpu_floor_mutex);
 }
 
+static void check_power_collapse_modes(struct dcvs_core *core)
+{
+	struct msm_dcvs_algo_param *params;
+
+	params = &core_list[CPU_OFFSET + num_online_cpus() - 1].algo_param;
+
+	if (core->actual_freq >= params->disable_pc_threshold)
+		core->idle_enable(core->type_core_num,
+				  MSM_DCVS_DISABLE_HIGH_LATENCY_MODES);
+	else
+		core->idle_enable(core->type_core_num,
+				  MSM_DCVS_ENABLE_HIGH_LATENCY_MODES);
+}
+
 static int __msm_dcvs_change_freq(struct dcvs_core *core)
 {
 	int ret = 0;
@@ -355,16 +370,11 @@
 	core->freq_change_us = (uint32_t)ktime_to_us(
 					ktime_sub(ktime_get(), time_start));
 
-	/**
-	 * Disable low power modes if the actual frequency is >
-	 * disable_pc_threshold.
-	 */
-	if (core->actual_freq > core->algo_param.disable_pc_threshold) {
-		core->idle_enable(core->type_core_num,
-				MSM_DCVS_DISABLE_HIGH_LATENCY_MODES);
-	} else if (core->actual_freq <= core->algo_param.disable_pc_threshold) {
-		core->idle_enable(core->type_core_num,
-				MSM_DCVS_ENABLE_HIGH_LATENCY_MODES);
+	if (core->type == MSM_DCVS_CORE_TYPE_CPU &&
+	    core->type_core_num == 0) {
+		mutex_lock(&param_update_mutex);
+		check_power_collapse_modes(core);
+		mutex_unlock(&param_update_mutex);
 	}
 
 	/**
@@ -556,7 +566,6 @@
 int msm_dcvs_update_algo_params(void)
 {
 	static struct msm_dcvs_algo_param curr_params;
-	static DEFINE_MUTEX(param_update_mutex);
 	struct msm_dcvs_algo_param *new_params;
 	int cpu, ret = 0;
 
@@ -566,6 +575,7 @@
 	if (memcmp(&curr_params, new_params,
 		   sizeof(struct msm_dcvs_algo_param))) {
 		for_each_possible_cpu(cpu) {
+			struct dcvs_core *core = &core_list[CPU_OFFSET + cpu];
 			ret = msm_dcvs_scm_set_algo_params(CPU_OFFSET + cpu,
 							   new_params);
 			if (ret) {
@@ -574,6 +584,8 @@
 				mutex_unlock(&param_update_mutex);
 				return ret;
 			}
+			if (cpu == 0)
+				check_power_collapse_modes(core);
 		}
 		memcpy(&curr_params, new_params,
 		       sizeof(struct msm_dcvs_algo_param));
@@ -706,7 +718,7 @@
 DCVS_ALGO_PARAM(slack_time_min_us)
 DCVS_ALGO_PARAM(slack_time_max_us)
 DCVS_ALGO_PARAM(slack_weight_thresh_pct)
-DCVS_ALGO_PARAM(ss_iobusy_conv)
+DCVS_ALGO_PARAM(ss_no_corr_below_freq)
 DCVS_ALGO_PARAM(ss_win_size_min_us)
 DCVS_ALGO_PARAM(ss_win_size_max_us)
 DCVS_ALGO_PARAM(ss_util_pct)
@@ -892,7 +904,7 @@
 	DCVS_RW_ATTRIB(8, slack_weight_thresh_pct);
 	DCVS_RW_ATTRIB(9, slack_time_min_us);
 	DCVS_RW_ATTRIB(10, slack_time_max_us);
-	DCVS_RW_ATTRIB(11, ss_iobusy_conv);
+	DCVS_RW_ATTRIB(11, ss_no_corr_below_freq);
 	DCVS_RW_ATTRIB(12, ss_win_size_min_us);
 	DCVS_RW_ATTRIB(13, ss_win_size_max_us);
 	DCVS_RW_ATTRIB(14, ss_util_pct);
diff --git a/arch/arm/mach-msm/msm_mpdecision.c b/arch/arm/mach-msm/msm_mpdecision.c
index 910804b..746bbe8 100644
--- a/arch/arm/mach-msm/msm_mpdecision.c
+++ b/arch/arm/mach-msm/msm_mpdecision.c
@@ -41,12 +41,14 @@
 #include <trace/events/mpdcvs_trace.h>
 
 #define DEFAULT_RQ_AVG_POLL_MS    (1)
+#define DEFAULT_RQ_AVG_DIVIDE    (25)
 
 struct mpd_attrib {
 	struct kobj_attribute	enabled;
 	struct kobj_attribute	rq_avg_poll_ms;
-	struct kobj_attribute iowait_threshold_pct;
+	struct kobj_attribute	iowait_threshold_pct;
 
+	struct kobj_attribute	rq_avg_divide;
 	struct kobj_attribute	em_win_size_min_us;
 	struct kobj_attribute	em_win_size_max_us;
 	struct kobj_attribute	em_max_util_pct;
@@ -75,6 +77,7 @@
 	atomic_t			algo_cpu_mask;
 	uint32_t			rq_avg_poll_ms;
 	uint32_t			iowait_threshold_pct;
+	uint32_t			rq_avg_divide;
 	ktime_t				next_update;
 	uint32_t			slack_us;
 	struct msm_mpd_algo_param	mp_param;
@@ -125,20 +128,19 @@
 static int num_present_hundreds;
 static ktime_t last_down_time;
 
-#define RQ_AVG_INSIGNIFICANT_BITS	3
 static bool ok_to_update_tz(int nr, int last_nr)
 {
 	/*
 	 * Exclude unnecessary TZ reports if run queue haven't changed much from
-	 * the last reported value. The left shift by INSIGNIFICANT_BITS is to
+	 * the last reported value. The divison by rq_avg_divide is to
 	 * filter out small changes in the run queue average which won't cause
 	 * a online cpu mask change. Also if the cpu online count does not match
 	 * the count requested by TZ and we are not in the process of bringing
 	 * cpus online as indicated by a HPUPDATE_IN_PROGRESS in msm_mpd.hpdata
 	 */
 	return
-	(((nr >> RQ_AVG_INSIGNIFICANT_BITS)
-				!= (last_nr >> RQ_AVG_INSIGNIFICANT_BITS))
+	(((nr / msm_mpd.rq_avg_divide)
+				!= (last_nr / msm_mpd.rq_avg_divide))
 	|| ((hweight32(atomic_read(&msm_mpd.algo_cpu_mask))
 				!= num_online_cpus())
 		&& (msm_mpd.hpupdate != HPUPDATE_IN_PROGRESS)));
@@ -509,6 +511,20 @@
 	return 0;
 }
 
+static int msm_mpd_set_rq_avg_divide(uint32_t val)
+{
+	/*
+	 * No need to do anything. New value will be used next time
+	 * the decision is made as to whether to update tz.
+	 */
+
+	if (val == 0)
+		return -EINVAL;
+
+	msm_mpd.rq_avg_divide = val;
+	return 0;
+}
+
 #define MPD_ALGO_PARAM(_name, _param) \
 static ssize_t msm_mpd_attr_##_name##_show(struct kobject *kobj, \
 			struct kobj_attribute *attr, char *buf) \
@@ -580,6 +596,7 @@
 MPD_PARAM(enabled, msm_mpd.enabled);
 MPD_PARAM(rq_avg_poll_ms, msm_mpd.rq_avg_poll_ms);
 MPD_PARAM(iowait_threshold_pct, msm_mpd.iowait_threshold_pct);
+MPD_PARAM(rq_avg_divide, msm_mpd.rq_avg_divide);
 MPD_ALGO_PARAM(em_win_size_min_us, msm_mpd.mp_param.em_win_size_min_us);
 MPD_ALGO_PARAM(em_win_size_max_us, msm_mpd.mp_param.em_win_size_max_us);
 MPD_ALGO_PARAM(em_max_util_pct, msm_mpd.mp_param.em_max_util_pct);
@@ -602,7 +619,7 @@
 {
 	struct kobject *module_kobj = NULL;
 	int ret = 0;
-	const int attr_count = 19;
+	const int attr_count = 20;
 	struct msm_mpd_algo_param *param = NULL;
 
 	param = pdev->dev.platform_data;
@@ -624,28 +641,30 @@
 	MPD_RW_ATTRIB(0, enabled);
 	MPD_RW_ATTRIB(1, rq_avg_poll_ms);
 	MPD_RW_ATTRIB(2, iowait_threshold_pct);
-	MPD_RW_ATTRIB(3, em_win_size_min_us);
-	MPD_RW_ATTRIB(4, em_win_size_max_us);
-	MPD_RW_ATTRIB(5, em_max_util_pct);
-	MPD_RW_ATTRIB(6, mp_em_rounding_point_min);
-	MPD_RW_ATTRIB(7, mp_em_rounding_point_max);
-	MPD_RW_ATTRIB(8, online_util_pct_min);
-	MPD_RW_ATTRIB(9, online_util_pct_max);
-	MPD_RW_ATTRIB(10, slack_time_min_us);
-	MPD_RW_ATTRIB(11, slack_time_max_us);
-	MPD_RW_ATTRIB(12, hp_up_max_ms);
-	MPD_RW_ATTRIB(13, hp_up_ms);
-	MPD_RW_ATTRIB(14, hp_up_count);
-	MPD_RW_ATTRIB(15, hp_dw_max_ms);
-	MPD_RW_ATTRIB(16, hp_dw_ms);
-	MPD_RW_ATTRIB(17, hp_dw_count);
+	MPD_RW_ATTRIB(3, rq_avg_divide);
+	MPD_RW_ATTRIB(4, em_win_size_min_us);
+	MPD_RW_ATTRIB(5, em_win_size_max_us);
+	MPD_RW_ATTRIB(6, em_max_util_pct);
+	MPD_RW_ATTRIB(7, mp_em_rounding_point_min);
+	MPD_RW_ATTRIB(8, mp_em_rounding_point_max);
+	MPD_RW_ATTRIB(9, online_util_pct_min);
+	MPD_RW_ATTRIB(10, online_util_pct_max);
+	MPD_RW_ATTRIB(11, slack_time_min_us);
+	MPD_RW_ATTRIB(12, slack_time_max_us);
+	MPD_RW_ATTRIB(13, hp_up_max_ms);
+	MPD_RW_ATTRIB(14, hp_up_ms);
+	MPD_RW_ATTRIB(15, hp_up_count);
+	MPD_RW_ATTRIB(16, hp_dw_max_ms);
+	MPD_RW_ATTRIB(17, hp_dw_ms);
+	MPD_RW_ATTRIB(18, hp_dw_count);
 
-	msm_mpd.attrib.attrib_group.attrs[18] = NULL;
+	msm_mpd.attrib.attrib_group.attrs[19] = NULL;
 	ret = sysfs_create_group(module_kobj, &msm_mpd.attrib.attrib_group);
 	if (ret)
 		pr_err("Unable to create sysfs objects :%d\n", ret);
 
 	msm_mpd.rq_avg_poll_ms = DEFAULT_RQ_AVG_POLL_MS;
+	msm_mpd.rq_avg_divide = DEFAULT_RQ_AVG_DIVIDE;
 
 	memcpy(&msm_mpd.mp_param, param, sizeof(struct msm_mpd_algo_param));
 
diff --git a/arch/arm/mach-msm/ocmem_rdm.c b/arch/arm/mach-msm/ocmem_rdm.c
index 818a20a..8962729 100644
--- a/arch/arm/mach-msm/ocmem_rdm.c
+++ b/arch/arm/mach-msm/ocmem_rdm.c
@@ -191,6 +191,7 @@
 	int table_end = 0;
 	int br_ctrl = 0;
 	int br_id = 0;
+	int client_id = 0;
 	int dm_ctrl = 0;
 	int i = 0;
 	int j = 0;
@@ -244,6 +245,8 @@
 	dm_ctrl |= (table_start << DM_TBL_START);
 	dm_ctrl |= (table_end << DM_TBL_END);
 
+	client_id = client_ctrl_id(id);
+	dm_ctrl |= (client_id << DM_CLIENT_SHIFT);
 	dm_ctrl |= (DM_BR_ID_LPASS << DM_BR_ID_SHIFT);
 	dm_ctrl |= (DM_BLOCK_256 << DM_BR_BLK_SHIFT);
 	dm_ctrl |= (direction << DM_DIR_SHIFT);
diff --git a/arch/arm/mach-msm/perf_event_msm_krait_l2.c b/arch/arm/mach-msm/perf_event_msm_krait_l2.c
index 103eef0..26c5e58 100644
--- a/arch/arm/mach-msm/perf_event_msm_krait_l2.c
+++ b/arch/arm/mach-msm/perf_event_msm_krait_l2.c
@@ -72,9 +72,11 @@
  */
 struct pmu_constraints {
 	u64 pmu_bitmap;
+	u8 codes[64];
 	raw_spinlock_t lock;
 } l2_pmu_constraints = {
 	.pmu_bitmap = 0,
+	.codes = {-1},
 	.lock = __RAW_SPIN_LOCK_UNLOCKED(l2_pmu_constraints.lock),
 };
 
@@ -335,8 +337,10 @@
 	int ctr = 0;
 
 	if (hwc->config_base == L2CYCLE_CTR_RAW_CODE) {
-		if (!test_and_set_bit(l2_cycle_ctr_idx, cpuc->used_mask))
-			return l2_cycle_ctr_idx;
+		if (test_and_set_bit(l2_cycle_ctr_idx, cpuc->used_mask))
+			return -EAGAIN;
+
+		return l2_cycle_ctr_idx;
 	}
 
 	for (ctr = 0; ctr < total_l2_ctrs - 1; ctr++) {
@@ -453,22 +457,48 @@
 {
 	u32 evt_type = event->attr.config & L2_EVT_MASK;
 	u8 reg   = (evt_type & 0x0F000) >> 12;
-	u8 group =  evt_type & 0x0000F;
+	u8 group = evt_type & 0x0000F;
+	u8 code = (evt_type & 0x00FF0) >> 4;
 	unsigned long flags;
 	u32 err = 0;
 	u64 bitmap_t;
+	u32 shift_idx;
+
+	/*
+	 * Cycle counter collision is detected in
+	 * get_event_idx().
+	 */
+	if (evt_type == L2CYCLE_CTR_RAW_CODE)
+		return err;
 
 	raw_spin_lock_irqsave(&l2_pmu_constraints.lock, flags);
 
-	bitmap_t = 1 << ((reg * 4) + group);
+	shift_idx = ((reg * 4) + group);
+
+	bitmap_t = 1 << shift_idx;
 
 	if (!(l2_pmu_constraints.pmu_bitmap & bitmap_t)) {
 		l2_pmu_constraints.pmu_bitmap |= bitmap_t;
+		l2_pmu_constraints.codes[shift_idx] = code;
 		goto out;
+	} else {
+		/*
+		 * If NRCCG's are identical,
+		 * its not column exclusion.
+		 */
+		if (l2_pmu_constraints.codes[shift_idx] != code)
+			err = -EPERM;
+		else
+			/*
+			 * If the event is counted in syswide mode
+			 * then we want to count only on one CPU
+			 * and set its filter to count from all.
+			 * This sets the event OFF on all but one
+			 * CPU.
+			 */
+			if (!(event->cpu < 0))
+				event->state = PERF_EVENT_STATE_OFF;
 	}
-
-	/* Bit is already set. Constraint failed. */
-	err = -EPERM;
 out:
 	raw_spin_unlock_irqrestore(&l2_pmu_constraints.lock, flags);
 	return err;
@@ -481,14 +511,20 @@
 	u8 group =  evt_type & 0x0000F;
 	unsigned long flags;
 	u64 bitmap_t;
+	u32 shift_idx;
 
 	raw_spin_lock_irqsave(&l2_pmu_constraints.lock, flags);
 
-	bitmap_t = 1 << ((reg * 4) + group);
+	shift_idx = ((reg * 4) + group);
+
+	bitmap_t = 1 << shift_idx;
 
 	/* Clear constraint bit. */
 	l2_pmu_constraints.pmu_bitmap &= ~bitmap_t;
 
+	/* Clear code. */
+	l2_pmu_constraints.codes[shift_idx] = -1;
+
 	raw_spin_unlock_irqrestore(&l2_pmu_constraints.lock, flags);
 	return 1;
 }
diff --git a/arch/arm/mach-msm/perf_event_msm_l2.c b/arch/arm/mach-msm/perf_event_msm_l2.c
index 2ad36df..c1b7d23 100644
--- a/arch/arm/mach-msm/perf_event_msm_l2.c
+++ b/arch/arm/mach-msm/perf_event_msm_l2.c
@@ -38,9 +38,11 @@
  */
 struct pmu_constraints {
 	u64 pmu_bitmap;
+	u8 codes[64];
 	raw_spinlock_t lock;
 } l2_pmu_constraints = {
 	.pmu_bitmap = 0,
+	.codes = {-1},
 	.lock = __RAW_SPIN_LOCK_UNLOCKED(l2_pmu_constraints.lock),
 };
 
@@ -667,9 +669,11 @@
 	int ctr = 0;
 
 	if (hwc->config_base == SCORPION_L2CYCLE_CTR_RAW_CODE) {
-		if (!test_and_set_bit(l2_cycle_ctr_idx,
+		if (test_and_set_bit(l2_cycle_ctr_idx,
 					cpuc->used_mask))
-			return l2_cycle_ctr_idx;
+			return -EAGAIN;
+
+		return l2_cycle_ctr_idx;
 	}
 
 	for (ctr = 0; ctr < total_l2_ctrs - 1; ctr++) {
@@ -792,25 +796,50 @@
 	u8 prefix = (evt_type & 0xF0000) >> 16;
 	u8 reg   = (evt_type & 0x0F000) >> 12;
 	u8 group =  evt_type & 0x0000F;
+	u8 code = (evt_type & 0x00FF0) >> 4;
 	unsigned long flags;
 	u32 err = 0;
 	u64 bitmap_t;
+	u32 shift_idx;
 
 	if (!prefix)
 		return 0;
+	/*
+	 * Cycle counter collision is detected in
+	 * get_event_idx().
+	 */
+	if (evt_type == SCORPION_L2CYCLE_CTR_RAW_CODE)
+		return err;
 
 	raw_spin_lock_irqsave(&l2_pmu_constraints.lock, flags);
 
-	bitmap_t = 1 << ((reg * 4) + group);
+	shift_idx = ((reg * 4) + group);
+
+	bitmap_t = 1 << shift_idx;
 
 	if (!(l2_pmu_constraints.pmu_bitmap & bitmap_t)) {
 		l2_pmu_constraints.pmu_bitmap |= bitmap_t;
+		l2_pmu_constraints.codes[shift_idx] = code;
 		goto out;
+	} else {
+		/*
+		 * If NRCCG's are identical,
+		 * its not column exclusion.
+		 */
+		if (l2_pmu_constraints.codes[shift_idx] != code)
+			err = -EPERM;
+		else
+			/*
+			 * If the event is counted in syswide mode
+			 * then we want to count only on one CPU
+			 * and set its filter to count from all.
+			 * This sets the event OFF on all but one
+			 * CPU.
+			 */
+			if (!(event->cpu < 0))
+				event->state = PERF_EVENT_STATE_OFF;
 	}
 
-	/* Bit is already set. Constraint failed. */
-	err = -EPERM;
-
 out:
 	raw_spin_unlock_irqrestore(&l2_pmu_constraints.lock, flags);
 	return err;
@@ -824,13 +853,16 @@
 	u8 group =  evt_type & 0x0000F;
 	unsigned long flags;
 	u64 bitmap_t;
+	u32 shift_idx;
 
 	if (!prefix)
 		return 0;
 
 	raw_spin_lock_irqsave(&l2_pmu_constraints.lock, flags);
 
-	bitmap_t = 1 << ((reg * 4) + group);
+	shift_idx = ((reg * 4) + group);
+
+	bitmap_t = 1 << shift_idx;
 
 	/* Clear constraint bit. */
 	l2_pmu_constraints.pmu_bitmap &= ~bitmap_t;
diff --git a/arch/arm/mach-msm/peripheral-loader.c b/arch/arm/mach-msm/peripheral-loader.c
index e3a3563..88aae81 100644
--- a/arch/arm/mach-msm/peripheral-loader.c
+++ b/arch/arm/mach-msm/peripheral-loader.c
@@ -28,17 +28,24 @@
 #include <linux/msm_ion.h>
 #include <linux/list.h>
 #include <linux/list_sort.h>
+#include <linux/idr.h>
 
 #include <asm/uaccess.h>
 #include <asm/setup.h>
+#include <asm-generic/io-64-nonatomic-lo-hi.h>
+
+#include <mach/msm_iomap.h>
 
 #include "peripheral-loader.h"
+#include "ramdump.h"
 
 #define pil_err(desc, fmt, ...)						\
 	dev_err(desc->dev, "%s: " fmt, desc->name, ##__VA_ARGS__)
 #define pil_info(desc, fmt, ...)					\
 	dev_info(desc->dev, "%s: " fmt, desc->name, ##__VA_ARGS__)
 
+#define PIL_IMAGE_INFO_BASE	(MSM_IMEM_BASE + 0x94c)
+
 /**
  * proxy_timeout - Override for proxy vote timeouts
  * -1: Use driver-specified timeout
@@ -80,6 +87,18 @@
 };
 
 /**
+ * struct pil_image_info - information in IMEM about image and where it is loaded
+ * @name: name of image (may or may not be NULL terminated)
+ * @start: indicates physical address where image starts (little endian)
+ * @size: size of image (little endian)
+ */
+struct pil_image_info {
+	char name[8];
+	__le64 start;
+	__le32 size;
+} __attribute__((__packed__));
+
+/**
  * struct pil_priv - Private state for a pil_desc
  * @proxy: work item used to run the proxy unvoting routine
  * @wlock: wakelock to prevent suspend during pil_boot
@@ -110,8 +129,46 @@
 	phys_addr_t region_start;
 	phys_addr_t region_end;
 	struct ion_handle *region;
+	struct pil_image_info __iomem *info;
+	int id;
 };
 
+/**
+ * pil_do_ramdump() - Ramdump an image
+ * @desc: descriptor from pil_desc_init()
+ * @ramdump_dev: ramdump device returned from create_ramdump_device()
+ *
+ * Calls the ramdump API with a list of segments generated from the addresses
+ * that the descriptor corresponds to.
+ */
+int pil_do_ramdump(struct pil_desc *desc, void *ramdump_dev)
+{
+	struct pil_priv *priv = desc->priv;
+	struct pil_seg *seg;
+	int count = 0, ret;
+	struct ramdump_segment *ramdump_segs, *s;
+
+	list_for_each_entry(seg, &priv->segs, list)
+		count++;
+
+	ramdump_segs = kmalloc_array(count, sizeof(*ramdump_segs), GFP_KERNEL);
+	if (!ramdump_segs)
+		return -ENOMEM;
+
+	s = ramdump_segs;
+	list_for_each_entry(seg, &priv->segs, list) {
+		s->address = seg->paddr;
+		s->size = seg->sz;
+		s++;
+	}
+
+	ret = do_elf_ramdump(ramdump_dev, ramdump_segs, count);
+	kfree(ramdump_segs);
+
+	return ret;
+}
+EXPORT_SYMBOL(pil_do_ramdump);
+
 static struct ion_client *ion;
 
 /**
@@ -337,6 +394,10 @@
 		priv->base_addr = min_addr_n;
 	}
 
+	writeq(priv->region_start, &priv->info->start);
+	writel_relaxed(priv->region_end - priv->region_start,
+			&priv->info->size);
+
 	return ret;
 }
 
@@ -380,6 +441,9 @@
 	struct pil_priv *priv = desc->priv;
 	struct pil_seg *p, *tmp;
 
+	writeq(0, &priv->info->start);
+	writel_relaxed(0, &priv->info->size);
+
 	if (priv->region)
 		ion_free(ion, priv->region);
 	priv->region = NULL;
@@ -593,6 +657,8 @@
 }
 EXPORT_SYMBOL(pil_shutdown);
 
+static DEFINE_IDA(pil_ida);
+
 /**
  * pil_desc_init() - Initialize a pil descriptor
  * @desc: descriptor to intialize
@@ -605,6 +671,9 @@
 int pil_desc_init(struct pil_desc *desc)
 {
 	struct pil_priv *priv;
+	int id;
+	void __iomem *addr;
+	size_t len;
 
 	/* Ignore users who don't make any sense */
 	WARN(desc->ops->proxy_unvote && !desc->proxy_timeout,
@@ -619,6 +688,18 @@
 	desc->priv = priv;
 	priv->desc = desc;
 
+	priv->id = id = ida_simple_get(&pil_ida, 0, 10, GFP_KERNEL);
+	if (id < 0) {
+		kfree(priv);
+		return id;
+	}
+	addr = PIL_IMAGE_INFO_BASE + sizeof(struct pil_image_info) * id;
+	priv->info = (struct pil_image_info __iomem *)addr;
+
+	len = min(strlen(desc->name), sizeof(priv->info->name));
+	memset_io(priv->info->name, 0, sizeof(priv->info->name));
+	memcpy_toio(priv->info->name, desc->name, len);
+
 	snprintf(priv->wname, sizeof(priv->wname), "pil-%s", desc->name);
 	wake_lock_init(&priv->wlock, WAKE_LOCK_SUSPEND, priv->wname);
 	INIT_DELAYED_WORK(&priv->proxy, pil_proxy_work);
@@ -637,6 +718,7 @@
 	struct pil_priv *priv = desc->priv;
 
 	if (priv) {
+		ida_simple_remove(&pil_ida, priv->id);
 		flush_delayed_work(&priv->proxy);
 		wake_lock_destroy(&priv->wlock);
 	}
diff --git a/arch/arm/mach-msm/peripheral-loader.h b/arch/arm/mach-msm/peripheral-loader.h
index 1c2faf7..8442289 100644
--- a/arch/arm/mach-msm/peripheral-loader.h
+++ b/arch/arm/mach-msm/peripheral-loader.h
@@ -65,6 +65,7 @@
 extern void pil_shutdown(struct pil_desc *desc);
 extern void pil_desc_release(struct pil_desc *desc);
 extern phys_addr_t pil_get_entry_addr(struct pil_desc *desc);
+extern int pil_do_ramdump(struct pil_desc *desc, void *ramdump_dev);
 #else
 static inline int pil_desc_init(struct pil_desc *desc) { return 0; }
 static inline int pil_boot(struct pil_desc *desc) { return 0; }
@@ -74,6 +75,10 @@
 {
 	return 0;
 }
+static inline int pil_do_ramdump(struct pil_desc *desc, void *ramdump_dev)
+{
+	return 0;
+}
 #endif
 
 #endif
diff --git a/arch/arm/mach-msm/pil-dsps.c b/arch/arm/mach-msm/pil-dsps.c
index 519e1c9..d315d82 100644
--- a/arch/arm/mach-msm/pil-dsps.c
+++ b/arch/arm/mach-msm/pil-dsps.c
@@ -48,7 +48,6 @@
 	void __iomem *ppss_base;
 
 	void *ramdump_dev;
-	struct ramdump_segment fw_ramdump_segments[4];
 
 	void *smem_ramdump_dev;
 	struct ramdump_segment smem_ramdump_segments[1];
@@ -212,16 +211,13 @@
 	if (!enable)
 		return 0;
 
-	ret = do_ramdump(drv->ramdump_dev,
-		drv->fw_ramdump_segments,
-		ARRAY_SIZE(drv->fw_ramdump_segments));
+	ret = pil_do_ramdump(&drv->desc, drv->ramdump_dev);
 	if (ret < 0) {
 		pr_err("%s: Unable to dump DSPS memory (rc = %d).\n",
 		       __func__, ret);
 		return ret;
 	}
-	ret = do_ramdump(drv->smem_ramdump_dev,
-		drv->smem_ramdump_segments,
+	ret = do_elf_ramdump(drv->smem_ramdump_dev, drv->smem_ramdump_segments,
 		ARRAY_SIZE(drv->smem_ramdump_segments));
 	if (ret < 0) {
 		pr_err("%s: Unable to dump smem memory (rc = %d).\n",
@@ -293,14 +289,6 @@
 	if (ret)
 		return ret;
 
-	drv->fw_ramdump_segments[0].address = 0x12000000;
-	drv->fw_ramdump_segments[0].size = 0x28000;
-	drv->fw_ramdump_segments[1].address = 0x12040000;
-	drv->fw_ramdump_segments[1].size = 0x4000;
-	drv->fw_ramdump_segments[2].address = 0x12800000;
-	drv->fw_ramdump_segments[2].size = 0x4000;
-	drv->fw_ramdump_segments[3].address = 0x8fe00000;
-	drv->fw_ramdump_segments[3].size = 0x100000;
 	drv->ramdump_dev = create_ramdump_device("dsps", &pdev->dev);
 	if (!drv->ramdump_dev) {
 		ret = -ENOMEM;
diff --git a/arch/arm/mach-msm/pil-gss.c b/arch/arm/mach-msm/pil-gss.c
index a6d13d0..f4d4449 100644
--- a/arch/arm/mach-msm/pil-gss.c
+++ b/arch/arm/mach-msm/pil-gss.c
@@ -404,11 +404,6 @@
 	smsm_reset_modem(SMSM_RESET);
 }
 
-/* FIXME: Get address, size from PIL */
-static struct ramdump_segment gss_segments[] = {
-	{0x89000000, 0x00D00000}
-};
-
 static struct ramdump_segment smem_segments[] = {
 	{0x80000000, 0x00200000},
 };
@@ -418,20 +413,20 @@
 	int ret;
 	struct gss_data *drv = container_of(desc, struct gss_data, subsys_desc);
 
-	if (enable) {
-		ret = do_ramdump(drv->ramdump_dev, gss_segments,
-				ARRAY_SIZE(gss_segments));
-		if (ret < 0) {
-			pr_err("Unable to dump gss memory\n");
-			return ret;
-		}
+	if (!enable)
+		return 0;
 
-		ret = do_ramdump(drv->smem_ramdump_dev, smem_segments,
-			ARRAY_SIZE(smem_segments));
-		if (ret < 0) {
-			pr_err("Unable to dump smem memory (rc = %d).\n", ret);
-			return ret;
-		}
+	ret = pil_do_ramdump(&drv->pil_desc, drv->ramdump_dev);
+	if (ret < 0) {
+		pr_err("Unable to dump gss memory\n");
+		return ret;
+	}
+
+	ret = do_elf_ramdump(drv->smem_ramdump_dev, smem_segments,
+		ARRAY_SIZE(smem_segments));
+	if (ret < 0) {
+		pr_err("Unable to dump smem memory (rc = %d).\n", ret);
+		return ret;
 	}
 
 	return 0;
diff --git a/arch/arm/mach-msm/pil-modem.c b/arch/arm/mach-msm/pil-modem.c
index d3c832b..3546705 100644
--- a/arch/arm/mach-msm/pil-modem.c
+++ b/arch/arm/mach-msm/pil-modem.c
@@ -393,21 +393,15 @@
 	return ret;
 }
 
-/* FIXME: Get address, size from PIL */
-static struct ramdump_segment modem_segments[] = {
-	{ 0x42F00000, 0x46000000 - 0x42F00000 },
-};
-
 static int modem_ramdump(int enable, const struct subsys_desc *subsys)
 {
 	struct modem_data *drv;
 
 	drv = container_of(subsys, struct modem_data, subsys_desc);
-	if (enable)
-		return do_ramdump(drv->ramdump_dev, modem_segments,
-				ARRAY_SIZE(modem_segments));
-	else
+	if (!enable)
 		return 0;
+
+	return pil_do_ramdump(&drv->pil_desc, drv->ramdump_dev);
 }
 
 static int __devinit pil_modem_driver_probe(struct platform_device *pdev)
diff --git a/arch/arm/mach-msm/pil-pronto.c b/arch/arm/mach-msm/pil-pronto.c
index 162a7f7..b457599 100644
--- a/arch/arm/mach-msm/pil-pronto.c
+++ b/arch/arm/mach-msm/pil-pronto.c
@@ -385,19 +385,14 @@
 		smsm_change_state(SMSM_APPS_STATE, SMSM_RESET, SMSM_RESET);
 }
 
-static struct ramdump_segment pronto_segments[] = {
-	{ 0x0D200000, 0x0D980000 - 0x0D200000 }
-};
-
 static int wcnss_ramdump(int enable, const struct subsys_desc *subsys)
 {
 	struct pronto_data *drv = subsys_to_drv(subsys);
 
-	if (enable)
-		return do_ramdump(drv->ramdump_dev, pronto_segments,
-				ARRAY_SIZE(pronto_segments));
-	else
+	if (!enable)
 		return 0;
+
+	return pil_do_ramdump(&drv->desc, drv->ramdump_dev);
 }
 
 static int __devinit pil_pronto_probe(struct platform_device *pdev)
diff --git a/arch/arm/mach-msm/pil-q6v3.c b/arch/arm/mach-msm/pil-q6v3.c
index d7e712c..1f53f17 100644
--- a/arch/arm/mach-msm/pil-q6v3.c
+++ b/arch/arm/mach-msm/pil-q6v3.c
@@ -279,22 +279,15 @@
 	return ret;
 }
 
-/* FIXME: Get address, size from PIL */
-static struct ramdump_segment q6_segments[] = {
-	{ 0x46700000, 0x47f00000 - 0x46700000 },
-	{ 0x28400000, 0x12800 }
-};
-
 static int lpass_q6_ramdump(int enable, const struct subsys_desc *subsys)
 {
 	struct q6v3_data *drv;
 
 	drv = container_of(subsys, struct q6v3_data, subsys_desc);
-	if (enable)
-		return do_ramdump(drv->ramdump_dev, q6_segments,
-				ARRAY_SIZE(q6_segments));
-	else
+	if (!enable)
 		return 0;
+
+	return pil_do_ramdump(&drv->pil_desc, drv->ramdump_dev);
 }
 
 static void lpass_q6_crash_shutdown(const struct subsys_desc *subsys)
diff --git a/arch/arm/mach-msm/pil-q6v4-lpass.c b/arch/arm/mach-msm/pil-q6v4-lpass.c
index 1e6c1f6..1387433 100644
--- a/arch/arm/mach-msm/pil-q6v4-lpass.c
+++ b/arch/arm/mach-msm/pil-q6v4-lpass.c
@@ -231,18 +231,14 @@
 	return ret;
 }
 
-static struct ramdump_segment segments[] = {
-	{0x8da00000, 0x8f200000 - 0x8da00000},
-	{0x28400000, 0x20000}
-};
-
 static int lpass_ramdump(int enable, const struct subsys_desc *subsys)
 {
 	struct lpass_q6v4 *drv = subsys_to_lpass(subsys);
 
 	if (!enable)
 		return 0;
-	return do_ramdump(drv->ramdump_dev, segments, ARRAY_SIZE(segments));
+
+	return pil_do_ramdump(&drv->q6.desc, drv->ramdump_dev);
 }
 
 static void lpass_crash_shutdown(const struct subsys_desc *subsys)
diff --git a/arch/arm/mach-msm/pil-q6v4-mss.c b/arch/arm/mach-msm/pil-q6v4-mss.c
index ee01f04..f2b090f 100644
--- a/arch/arm/mach-msm/pil-q6v4-mss.c
+++ b/arch/arm/mach-msm/pil-q6v4-mss.c
@@ -243,14 +243,6 @@
 	smsm_reset_modem(SMSM_RESET);
 }
 
-static struct ramdump_segment sw_segments[] = {
-	{0x89000000, 0x8D400000 - 0x89000000},
-};
-
-static struct ramdump_segment fw_segments[] = {
-	{0x8D400000, 0x8DA00000 - 0x8D400000},
-};
-
 static struct ramdump_segment smem_segments[] = {
 	{0x80000000, 0x00200000},
 };
@@ -263,17 +255,15 @@
 	if (!enable)
 		return 0;
 
-	ret = do_ramdump(drv->sw_ramdump_dev, sw_segments,
-		ARRAY_SIZE(sw_segments));
+	ret = pil_do_ramdump(&drv->q6_sw.desc, drv->sw_ramdump_dev);
 	if (ret < 0)
 		return ret;
 
-	ret = do_ramdump(drv->fw_ramdump_dev, fw_segments,
-		ARRAY_SIZE(fw_segments));
+	ret = pil_do_ramdump(&drv->q6_fw.desc, drv->fw_ramdump_dev);
 	if (ret < 0)
 		return ret;
 
-	ret = do_ramdump(drv->smem_ramdump_dev, smem_segments,
+	ret = do_elf_ramdump(drv->smem_ramdump_dev, smem_segments,
 		ARRAY_SIZE(smem_segments));
 	if (ret < 0)
 		return ret;
diff --git a/arch/arm/mach-msm/pil-q6v5-lpass.c b/arch/arm/mach-msm/pil-q6v5-lpass.c
index 662377d..5e03aa8 100644
--- a/arch/arm/mach-msm/pil-q6v5-lpass.c
+++ b/arch/arm/mach-msm/pil-q6v5-lpass.c
@@ -195,15 +195,10 @@
 								void *ss_handle)
 {
 	int ret;
-	switch (code) {
-	case SUBSYS_BEFORE_SHUTDOWN:
-		pr_debug("%s: M-Notify: Shutdown started\n", __func__);
-		ret = sysmon_send_event(SYSMON_SS_LPASS, "modem",
-				SUBSYS_BEFORE_SHUTDOWN);
-		if (ret < 0)
-			pr_err("%s: sysmon_send_event error %d", __func__, ret);
-		break;
-	}
+	pr_debug("%s: M-Notify: event %lu\n", __func__, code);
+	ret = sysmon_send_event(SYSMON_SS_LPASS, "modem", code);
+	if (ret < 0)
+		pr_err("%s: sysmon_send_event error %d", __func__, ret);
 	return NOTIFY_DONE;
 }
 
@@ -295,27 +290,19 @@
 {
 	struct lpass_data *drv = subsys_to_lpass(subsys);
 	int ret = 0;
-
-	if (get_restart_level() == RESET_SUBSYS_INDEPENDENT) {
-		pr_debug("%s: Wait for ADSP power up!", __func__);
-		msleep(10000);
-	}
-
 	ret = pil_boot(&drv->q6->desc);
 	enable_irq(drv->wdog_irq);
-
 	return ret;
 }
 
-static struct ramdump_segment segments = { 0xdc00000, 0x1800000 };
-
 static int adsp_ramdump(int enable, const struct subsys_desc *subsys)
 {
 	struct lpass_data *drv = subsys_to_lpass(subsys);
 
 	if (!enable)
 		return 0;
-	return do_ramdump(drv->ramdump_dev, &segments, 1);
+
+	return pil_do_ramdump(&drv->q6->desc, drv->ramdump_dev);
 }
 
 static void adsp_crash_shutdown(const struct subsys_desc *subsys)
diff --git a/arch/arm/mach-msm/pil-q6v5-mss.c b/arch/arm/mach-msm/pil-q6v5-mss.c
index b8309e5..ed85c95 100644
--- a/arch/arm/mach-msm/pil-q6v5-mss.c
+++ b/arch/arm/mach-msm/pil-q6v5-mss.c
@@ -496,10 +496,6 @@
 	smsm_reset_modem(SMSM_RESET);
 }
 
-static struct ramdump_segment modem_segments[] = {
-	{0x08400000, 0x0D100000 - 0x08400000},
-};
-
 static struct ramdump_segment smem_segments[] = {
 	{0x0FA00000, 0x0FC00000 - 0x0FA00000},
 };
@@ -516,14 +512,13 @@
 	if (ret)
 		return ret;
 
-	ret = do_ramdump(drv->ramdump_dev, modem_segments,
-				ARRAY_SIZE(modem_segments));
+	ret = pil_do_ramdump(&drv->q6->desc, drv->ramdump_dev);
 	if (ret < 0) {
 		pr_err("Unable to dump modem fw memory (rc = %d).\n", ret);
 		goto out;
 	}
 
-	ret = do_ramdump(drv->smem_ramdump_dev, smem_segments,
+	ret = do_elf_ramdump(drv->smem_ramdump_dev, smem_segments,
 		ARRAY_SIZE(smem_segments));
 	if (ret < 0) {
 		pr_err("Unable to dump smem memory (rc = %d).\n", ret);
diff --git a/arch/arm/mach-msm/pil-riva.c b/arch/arm/mach-msm/pil-riva.c
index 74fae98..96b9882 100644
--- a/arch/arm/mach-msm/pil-riva.c
+++ b/arch/arm/mach-msm/pil-riva.c
@@ -412,26 +412,16 @@
 	return ret;
 }
 
-/*
- * 7MB RAM segments for Riva SS;
- * Riva 1.1 0x8f000000 - 0x8f700000
- * Riva 1.0 0x8f200000 - 0x8f700000
- */
-static struct ramdump_segment riva_segments[] = {
-	{0x8f000000, 0x8f700000 - 0x8f000000}
-};
-
 static int riva_ramdump(int enable, const struct subsys_desc *desc)
 {
 	struct riva_data *drv;
 
 	drv = container_of(desc, struct riva_data, subsys_desc);
 
-	if (enable)
-		return do_ramdump(drv->ramdump_dev, riva_segments,
-				ARRAY_SIZE(riva_segments));
-	else
+	if (!enable)
 		return 0;
+
+	return pil_do_ramdump(&drv->pil_desc, drv->ramdump_dev);
 }
 
 /* Riva crash handler */
diff --git a/arch/arm/mach-msm/pil-venus.c b/arch/arm/mach-msm/pil-venus.c
index 103fd9f..eb222e3 100644
--- a/arch/arm/mach-msm/pil-venus.c
+++ b/arch/arm/mach-msm/pil-venus.c
@@ -33,6 +33,7 @@
 
 #include "peripheral-loader.h"
 #include "scm-pas.h"
+#include "ramdump.h"
 
 /* VENUS WRAPPER registers */
 #define VENUS_WRAPPER_CLOCK_CONFIG			0x4
@@ -76,6 +77,7 @@
 	struct iommu_domain *iommu_fw_domain;
 	int venus_domain_num;
 	bool is_booted;
+	void *ramdump_dev;
 	u32 fw_sz;
 	u32 fw_min_paddr;
 	u32 fw_max_paddr;
@@ -459,6 +461,29 @@
 	pil_shutdown(&drv->desc);
 }
 
+static int venus_shutdown(const struct subsys_desc *desc)
+{
+	struct venus_data *drv = subsys_to_drv(desc);
+	pil_shutdown(&drv->desc);
+	return 0;
+}
+
+static int venus_powerup(const struct subsys_desc *desc)
+{
+	struct venus_data *drv = subsys_to_drv(desc);
+	return pil_boot(&drv->desc);
+}
+
+static int venus_ramdump(int enable, const struct subsys_desc *desc)
+{
+	struct venus_data *drv = subsys_to_drv(desc);
+
+	if (!enable)
+		return 0;
+
+	return pil_do_ramdump(&drv->desc, drv->ramdump_dev);
+}
+
 static int __devinit pil_venus_probe(struct platform_device *pdev)
 {
 	struct venus_data *drv;
@@ -524,23 +549,34 @@
 		dev_info(&pdev->dev, "using non-secure boot\n");
 	}
 
+	drv->ramdump_dev = create_ramdump_device("venus", &pdev->dev);
+	if (!drv->ramdump_dev)
+		return -ENOMEM;
+
 	rc = pil_desc_init(desc);
 	if (rc)
-		return rc;
+		goto err_ramdump;
 
 	drv->subsys_desc.name = desc->name;
 	drv->subsys_desc.owner = THIS_MODULE;
 	drv->subsys_desc.dev = &pdev->dev;
 	drv->subsys_desc.start = venus_start;
 	drv->subsys_desc.stop = venus_stop;
+	drv->subsys_desc.shutdown = venus_shutdown;
+	drv->subsys_desc.powerup = venus_powerup;
+	drv->subsys_desc.ramdump = venus_ramdump;
 
 	drv->subsys = subsys_register(&drv->subsys_desc);
 	if (IS_ERR(drv->subsys)) {
-		pil_desc_release(desc);
-		return PTR_ERR(drv->subsys);
+		rc = PTR_ERR(drv->subsys);
+		goto err_subsys;
 	}
-
-	return 0;
+	return rc;
+err_subsys:
+	pil_desc_release(desc);
+err_ramdump:
+	destroy_ramdump_device(drv->ramdump_dev);
+	return rc;
 }
 
 static int __devexit pil_venus_remove(struct platform_device *pdev)
diff --git a/arch/arm/mach-msm/platsmp-8625.c b/arch/arm/mach-msm/platsmp-8625.c
index 0e75cae..bbb5a51 100644
--- a/arch/arm/mach-msm/platsmp-8625.c
+++ b/arch/arm/mach-msm/platsmp-8625.c
@@ -20,23 +20,16 @@
 #include <linux/interrupt.h>
 #include <linux/irq.h>
 
-#include <asm/cacheflush.h>
 #include <asm/hardware/gic.h>
-#include <asm/hardware/cache-l2x0.h>
 #include <asm/smp_scu.h>
 #include <asm/unified.h>
 #include <mach/msm_iomap.h>
 #include "pm.h"
+#include "platsmp.h"
 
 #define CORE_RESET_BASE		0xA8600590
 #define MSM_CORE_STATUS_MSK	0x02800000
 
-/*
- * control for which core is the next to come out of the secondary
- * boot "holding pen"
- */
-volatile int pen_release = -1;
-
 static DEFINE_PER_CPU(bool, cold_boot_done);
 
 struct per_cpu_data {
@@ -48,22 +41,8 @@
 
 static uint32_t *msm8625_boot_vector;
 
-
 static struct per_cpu_data cpu_data[CONFIG_NR_CPUS];
 
-/*
- * Write pen_release in a way that is guaranteed to be visible to all
- * observers, irrespective of whether they're taking part in coherency
- * or not.  This is necessary for the hotplug code to work reliably.
- */
-static void __cpuinit write_pen_release(int val)
-{
-	pen_release = val;
-	smp_wmb();
-	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
-	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
-}
-
 static void __iomem *scu_base_addr(void)
 {
 	return MSM_SCU_BASE;
@@ -101,10 +80,8 @@
 	local_irq_enable();
 }
 
-void __cpuinit platform_secondary_init(unsigned int cpu)
+void __cpuinit msm8625_platform_secondary_init(unsigned int cpu)
 {
-	pr_debug("CPU%u: Booted secondary processor\n", cpu);
-
 	WARN_ON(msm_platform_secondary_init(cpu));
 
 	/*
@@ -134,7 +111,7 @@
 	spin_unlock(&boot_lock);
 }
 
-static int  __cpuinit msm8625_release_secondary(unsigned int cpu)
+static int __cpuinit msm8625_release_secondary(unsigned int cpu)
 {
 	void __iomem *base_ptr;
 	int value = 0;
@@ -178,7 +155,7 @@
 	return cpu_data[cpu].reset_core_base;
 }
 
-int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
+int __cpuinit msm8625_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
 	unsigned long timeout;
 
@@ -245,7 +222,7 @@
  * Initialise the CPU possible map early - this describes the CPUs
  * which may be present or become present in the system.
  */
-void __init smp_init_cpus(void)
+void __init msm8625_smp_init_cpus(void)
 {
 	void __iomem *scu_base = scu_base_addr();
 
@@ -298,18 +275,11 @@
 	msm8625_boot_vector[1] = entry;
 }
 
-void __init platform_smp_prepare_cpus(unsigned int max_cpus)
+void __init msm8625_platform_smp_prepare_cpus(unsigned int max_cpus)
 {
-	int i, cpu, value;
+	int cpu, value;
 	void __iomem *cpu_ptr;
 
-	/*
-	 * Initialise the present map, which describes the set of CPUs
-	 * actually populated at the present time.
-	 */
-	for (i = 0; i < max_cpus; i++)
-		set_cpu_present(i, true);
-
 	scu_enable(scu_base_addr());
 
 	cpu_ptr = ioremap_nocache(MSM8625_CPU_PHYS, SZ_8);
@@ -350,3 +320,13 @@
 
 	}
 }
+
+struct smp_operations msm8625_smp_ops __initdata = {
+	.smp_init_cpus = msm8625_smp_init_cpus,
+	.smp_prepare_cpus = msm8625_platform_smp_prepare_cpus,
+	.smp_secondary_init = msm8625_platform_secondary_init,
+	.smp_boot_secondary = msm8625_boot_secondary,
+	.cpu_kill = platform_cpu_kill,
+	.cpu_die = platform_cpu_die,
+	.cpu_disable = platform_cpu_disable
+};
diff --git a/arch/arm/mach-msm/platsmp-8910.c b/arch/arm/mach-msm/platsmp-8910.c
deleted file mode 100644
index af2f496..0000000
--- a/arch/arm/mach-msm/platsmp-8910.c
+++ /dev/null
@@ -1,190 +0,0 @@
-/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/init.h>
-#include <linux/errno.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/jiffies.h>
-#include <linux/smp.h>
-#include <linux/io.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-
-#include <asm/cacheflush.h>
-#include <asm/smp_plat.h>
-#include <asm/hardware/gic.h>
-#include <mach/msm_iomap.h>
-#include "pm.h"
-
-#define BOOT_REMAP_ENABLE 0x01
-
-/*
- * control for which core is the next to come out of the secondary
- * boot "holding pen"
- */
-volatile int pen_release = -1;
-
-/*
- * Write pen_release in a way that is guaranteed to be visible to all
- * observers, irrespective of whether they're taking part in coherency
- * or not.  This is necessary for the hotplug code to work reliably.
- */
-static void __cpuinit write_pen_release(int val)
-{
-	pen_release = val;
-	smp_wmb();
-	__cpuc_flush_dcache_area((void *)&pen_release, sizeof(pen_release));
-	outer_clean_range(__pa(&pen_release), __pa(&pen_release + 1));
-}
-
-static DEFINE_SPINLOCK(boot_lock);
-
-void __cpuinit platform_secondary_init(unsigned int cpu)
-{
-	WARN_ON(msm_platform_secondary_init(cpu));
-
-	/*
-	 * if any interrupts are already enabled for the primary
-	 * core (e.g. timer irq), then they will not have been enabled
-	 * for us: do so
-	 */
-	gic_secondary_init(0);
-
-	/*
-	 * let the primary processor know we're out of the
-	 * pen, then head off into the C entry point
-	 */
-	write_pen_release(-1);
-
-	/*
-	 * Synchronise with the boot thread.
-	 */
-	spin_lock(&boot_lock);
-	spin_unlock(&boot_lock);
-}
-
-static int  __cpuinit release_secondary_sim(unsigned long base, int cpu)
-{
-	void *base_ptr;
-
-	base_ptr = ioremap_nocache(base + (cpu * 0x10000), SZ_4K);
-	if (!base_ptr) {
-		pr_err("Failed to release core %u\n", cpu);
-		return -ENODEV;
-	}
-
-	writel_relaxed(0x800, base_ptr+0x04);
-	writel_relaxed(0x3FFF, base_ptr+0x14);
-	mb();
-
-	return 0;
-}
-
-DEFINE_PER_CPU(int, cold_boot_done);
-
-int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
-{
-	unsigned long timeout;
-
-	preset_lpj = loops_per_jiffy;
-
-	if (per_cpu(cold_boot_done, cpu) == false) {
-		release_secondary_sim(0xF9088000, cpu);
-		per_cpu(cold_boot_done, cpu) = true;
-	}
-
-	/*
-	 * Set synchronisation state between this boot processor
-	 * and the secondary one
-	 */
-	spin_lock(&boot_lock);
-
-	/*
-	 * The secondary processor is waiting to be released from
-	 * the holding pen - release it, then wait for it to flag
-	 * that it has been released by resetting pen_release.
-	 *
-	 * Note that "pen_release" is the hardware CPU ID, whereas
-	 * "cpu" is Linux's internal ID.
-	 */
-	write_pen_release(cpu_logical_map(cpu));
-
-	/*
-	 * Send the secondary CPU a soft interrupt, thereby causing
-	 * the boot monitor to read the system wide flags register,
-	 * and branch to the address found there.
-	 */
-
-	gic_raise_softirq(cpumask_of(cpu), 1);
-
-	timeout = jiffies + (1 * HZ);
-	while (time_before(jiffies, timeout)) {
-		smp_rmb();
-		if (pen_release == -1)
-			break;
-
-		udelay(10);
-	}
-
-	/*
-	 * now the secondary core is starting up let it run its
-	 * calibrations, then wait for it to finish
-	 */
-	spin_unlock(&boot_lock);
-
-	return 0;
-}
-
-/*
- * Initialise the CPU possible map early - this describes the CPUs
- * which may be present or become present in the system
- */
-void __init smp_init_cpus(void)
-{
-	unsigned int i, ncores;
-
-	ncores = (__raw_readl(MSM_APCS_GCC_BASE + 0x30)) & 0xF;
-
-	for (i = 0; i < ncores; i++)
-		set_cpu_possible(i, true);
-
-	set_smp_cross_call(gic_raise_softirq);
-}
-
-void __init platform_smp_prepare_cpus(unsigned int max_cpus)
-{
-	int i;
-	void __iomem *remap_ptr;
-
-	/*
-	 * Initialise the present map, which describes the set of CPUs
-	 * actually populated at the present time
-	 */
-	for (i = 0; i < max_cpus; i++)
-		set_cpu_present(i, true);
-
-	/*
-	 * Enable boot remapping and write the address of secondary
-	 * startup into boot remapper register
-	 */
-	remap_ptr = ioremap_nocache(0xF9010000, SZ_4K); /* APCS_CFG_SECURE */
-	if (!remap_ptr) {
-		pr_err("Failed to ioremap for secondary cores\n");
-		return;
-	}
-
-	__raw_writel((virt_to_phys(msm_secondary_startup)|BOOT_REMAP_ENABLE),
-			(remap_ptr + 0x4));
-	mb();
-	iounmap(remap_ptr);
-}
diff --git a/arch/arm/mach-msm/platsmp.c b/arch/arm/mach-msm/platsmp.c
index b1d2464..80f6014 100644
--- a/arch/arm/mach-msm/platsmp.c
+++ b/arch/arm/mach-msm/platsmp.c
@@ -8,6 +8,7 @@
  * published by the Free Software Foundation.
  */
 
+#include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/init.h>
 #include <linux/cpumask.h>
@@ -27,14 +28,14 @@
 #include <mach/msm_iomap.h>
 
 #include "pm.h"
+#include "platsmp.h"
 #include "scm-boot.h"
 #include "spm.h"
 
 #define VDD_SC1_ARRAY_CLAMP_GFS_CTL 0x15A0
 #define SCSS_CPU1CORE_RESET 0xD80
 #define SCSS_DBG_STATUS_CORE_PWRDUP 0xE64
-
-extern void msm_secondary_startup(void);
+#define BOOT_REMAP_ENABLE 0x01
 
 /*
  * control for which core is the next to come out of the secondary
@@ -47,7 +48,7 @@
  * observers, irrespective of whether they're taking part in coherency
  * or not.  This is necessary for the hotplug code to work reliably.
  */
-static void __cpuinit write_pen_release(int val)
+void __cpuinit write_pen_release(int val)
 {
 	pen_release = val;
 	smp_wmb();
@@ -81,6 +82,20 @@
 	spin_unlock(&boot_lock);
 }
 
+static int __cpuinit release_secondary_sim(unsigned long base, int cpu)
+{
+	void *base_ptr = ioremap_nocache(base + (cpu * 0x10000), SZ_4K);
+	if (!base_ptr)
+		return -ENODEV;
+
+	writel_relaxed(0x800, base_ptr+0x04);
+	writel_relaxed(0x3FFF, base_ptr+0x14);
+
+	mb();
+	iounmap(base_ptr);
+	return 0;
+}
+
 static int __cpuinit scorpion_release_secondary(void)
 {
 	void *base_ptr = ioremap_nocache(0x00902000, SZ_4K*2);
@@ -97,23 +112,7 @@
 	return 0;
 }
 
-static int __cpuinit krait_release_secondary_sim(unsigned long base, int cpu)
-{
-	void *base_ptr = ioremap_nocache(base + (cpu * 0x10000), SZ_4K);
-	if (!base_ptr)
-		return -ENODEV;
-
-	if (machine_is_msm8974_sim() || machine_is_mpq8092_sim()) {
-		writel_relaxed(0x800, base_ptr+0x04);
-		writel_relaxed(0x3FFF, base_ptr+0x14);
-	}
-
-	mb();
-	iounmap(base_ptr);
-	return 0;
-}
-
-static int __cpuinit krait_release_secondary(unsigned long base, int cpu)
+static int __cpuinit msm8960_release_secondary(unsigned long base, int cpu)
 {
 	void *base_ptr = ioremap_nocache(base + (cpu * 0x10000), SZ_4K);
 	if (!base_ptr)
@@ -121,32 +120,30 @@
 
 	msm_spm_turn_on_cpu_rail(cpu);
 
-	if (cpu_is_krait_v1() || cpu_is_krait_v2()) {
-		writel_relaxed(0x109, base_ptr+0x04);
-		writel_relaxed(0x101, base_ptr+0x04);
-		mb();
-		ndelay(300);
-		writel_relaxed(0x121, base_ptr+0x04);
-	} else
-		writel_relaxed(0x021, base_ptr+0x04);
+	writel_relaxed(0x109, base_ptr+0x04);
+	writel_relaxed(0x101, base_ptr+0x04);
+	mb();
+	ndelay(300);
+
+	writel_relaxed(0x121, base_ptr+0x04);
 	mb();
 	udelay(2);
 
-	writel_relaxed(0x020, base_ptr+0x04);
+	writel_relaxed(0x120, base_ptr+0x04);
 	mb();
 	udelay(2);
 
-	writel_relaxed(0x000, base_ptr+0x04);
+	writel_relaxed(0x100, base_ptr+0x04);
 	mb();
 	udelay(100);
 
-	writel_relaxed(0x080, base_ptr+0x04);
+	writel_relaxed(0x180, base_ptr+0x04);
 	mb();
 	iounmap(base_ptr);
 	return 0;
 }
 
-static int __cpuinit krait_release_secondary_p3(unsigned long base, int cpu)
+static int __cpuinit msm8974_release_secondary(unsigned long base, int cpu)
 {
 	void *base_ptr = ioremap_nocache(base + (cpu * 0x10000), SZ_4K);
 	if (!base_ptr)
@@ -171,46 +168,13 @@
 	return 0;
 }
 
-static int __cpuinit release_secondary(unsigned int cpu)
-{
-	BUG_ON(cpu >= get_core_count());
-
-	if (machine_is_msm8974_rumi())
-		return 0;
-
-	if (cpu_is_msm8x60())
-		return scorpion_release_secondary();
-
-	if (machine_is_msm8974_sim() || machine_is_mpq8092_sim())
-		return krait_release_secondary_sim(0xf9088000, cpu);
-
-	if (soc_class_is_msm8960() || soc_class_is_msm8930() ||
-	    soc_class_is_apq8064())
-		return krait_release_secondary(0x02088000, cpu);
-
-	if (cpu_is_msm8974())
-		return krait_release_secondary_p3(0xf9088000, cpu);
-
-	WARN(1, "unknown CPU case in release_secondary\n");
-	return -EINVAL;
-}
-
-DEFINE_PER_CPU(int, cold_boot_done);
-
-int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
+static int __cpuinit release_from_pen(unsigned int cpu)
 {
 	unsigned long timeout;
 
-	pr_debug("Starting secondary CPU %d\n", cpu);
-
 	/* Set preset_lpj to avoid subsequent lpj recalculations */
 	preset_lpj = loops_per_jiffy;
 
-	if (per_cpu(cold_boot_done, cpu) == false) {
-		release_secondary(cpu);
-		per_cpu(cold_boot_done, cpu) = true;
-	}
-
 	/*
 	 * set synchronisation state between this boot processor
 	 * and the secondary one
@@ -251,11 +215,67 @@
 
 	return pen_release != -1 ? -ENOSYS : 0;
 }
+
+DEFINE_PER_CPU(int, cold_boot_done);
+
+int __cpuinit scorpion_boot_secondary(unsigned int cpu,
+				      struct task_struct *idle)
+{
+	pr_debug("Starting secondary CPU %d\n", cpu);
+
+	if (per_cpu(cold_boot_done, cpu) == false) {
+		scorpion_release_secondary();
+		per_cpu(cold_boot_done, cpu) = true;
+	}
+	return release_from_pen(cpu);
+}
+
+int __cpuinit msm8960_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	pr_debug("Starting secondary CPU %d\n", cpu);
+
+	if (per_cpu(cold_boot_done, cpu) == false) {
+		msm8960_release_secondary(0x02088000, cpu);
+		per_cpu(cold_boot_done, cpu) = true;
+	}
+	return release_from_pen(cpu);
+}
+
+int __cpuinit msm8974_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	pr_debug("Starting secondary CPU %d\n", cpu);
+
+	if (per_cpu(cold_boot_done, cpu) == false) {
+		if (machine_is_msm8974_sim() || machine_is_mpq8092_sim())
+			release_secondary_sim(0xf9088000, cpu);
+		else if (machine_is_msm8974_rumi())
+			return 0;
+		else
+			msm8974_release_secondary(0xf9088000, cpu);
+
+		per_cpu(cold_boot_done, cpu) = true;
+	}
+	return release_from_pen(cpu);
+}
+
+int __cpuinit arm_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	pr_debug("Starting secondary CPU %d\n", cpu);
+
+	if (per_cpu(cold_boot_done, cpu) == false) {
+		if (machine_is_msm8226_sim() || machine_is_msm8910_sim())
+			release_secondary_sim(0xf9088000, cpu);
+
+		per_cpu(cold_boot_done, cpu) = true;
+	}
+	return release_from_pen(cpu);
+}
+
 /*
  * Initialise the CPU possible map early - this describes the CPUs
  * which may be present or become present in the system.
  */
-void __init smp_init_cpus(void)
+static void __init msm_smp_init_cpus(void)
 {
 	unsigned int i, ncores = get_core_count();
 
@@ -271,6 +291,24 @@
 	set_smp_cross_call(gic_raise_softirq);
 }
 
+static void __init arm_smp_init_cpus(void)
+{
+	unsigned int i, ncores;
+
+	ncores = (__raw_readl(MSM_APCS_GCC_BASE + 0x30)) & 0xF;
+
+	if (ncores > nr_cpu_ids) {
+		pr_warn("SMP: %u cores greater than maximum (%u), clipping\n",
+			ncores, nr_cpu_ids);
+		ncores = nr_cpu_ids;
+	}
+
+	for (i = 0; i < ncores; i++)
+		set_cpu_possible(i, true);
+
+	set_smp_cross_call(gic_raise_softirq);
+}
+
 static int cold_boot_flags[] __initdata = {
 	0,
 	SCM_FLAG_COLDBOOT_CPU1,
@@ -278,7 +316,7 @@
 	SCM_FLAG_COLDBOOT_CPU3,
 };
 
-void __init platform_smp_prepare_cpus(unsigned int max_cpus)
+static void __init msm_platform_smp_prepare_cpus(unsigned int max_cpus)
 {
 	int cpu, map;
 	unsigned int flags = 0;
@@ -296,3 +334,61 @@
 	if (scm_set_boot_addr(virt_to_phys(msm_secondary_startup), flags))
 		pr_warn("Failed to set CPU boot address\n");
 }
+
+static void __init arm_platform_smp_prepare_cpus(unsigned int max_cpus)
+{
+	void *remap_ptr = ioremap_nocache(0xF9010000, SZ_4K);
+	if (!remap_ptr) {
+		pr_err("Failed to ioremap for secondary cores\n");
+		return;
+	}
+
+	/*
+	 * Write the address of secondary startup into boot remapper
+	 * register and enable boot remapping.
+	 */
+	__raw_writel((virt_to_phys(msm_secondary_startup)|BOOT_REMAP_ENABLE),
+			(remap_ptr + 0x4));
+	mb();
+	iounmap(remap_ptr);
+}
+
+struct smp_operations arm_smp_ops __initdata = {
+	.smp_init_cpus = arm_smp_init_cpus,
+	.smp_prepare_cpus = arm_platform_smp_prepare_cpus,
+	.smp_secondary_init = platform_secondary_init,
+	.smp_boot_secondary = arm_boot_secondary,
+	.cpu_kill = platform_cpu_kill,
+	.cpu_die = platform_cpu_die,
+	.cpu_disable = platform_cpu_disable
+};
+
+struct smp_operations msm8974_smp_ops __initdata = {
+	.smp_init_cpus = msm_smp_init_cpus,
+	.smp_prepare_cpus = msm_platform_smp_prepare_cpus,
+	.smp_secondary_init = platform_secondary_init,
+	.smp_boot_secondary = msm8974_boot_secondary,
+	.cpu_kill = platform_cpu_kill,
+	.cpu_die = platform_cpu_die,
+	.cpu_disable = platform_cpu_disable
+};
+
+struct smp_operations msm8960_smp_ops __initdata = {
+	.smp_init_cpus = msm_smp_init_cpus,
+	.smp_prepare_cpus = msm_platform_smp_prepare_cpus,
+	.smp_secondary_init = platform_secondary_init,
+	.smp_boot_secondary = msm8960_boot_secondary,
+	.cpu_kill = platform_cpu_kill,
+	.cpu_die = platform_cpu_die,
+	.cpu_disable = platform_cpu_disable
+};
+
+struct smp_operations scorpion_smp_ops __initdata = {
+	.smp_init_cpus = msm_smp_init_cpus,
+	.smp_prepare_cpus = msm_platform_smp_prepare_cpus,
+	.smp_secondary_init = platform_secondary_init,
+	.smp_boot_secondary = scorpion_boot_secondary,
+	.cpu_kill = platform_cpu_kill,
+	.cpu_die = platform_cpu_die,
+	.cpu_disable = platform_cpu_disable
+};
diff --git a/arch/arm/mach-msm/platsmp.h b/arch/arm/mach-msm/platsmp.h
new file mode 100644
index 0000000..1d176d2
--- /dev/null
+++ b/arch/arm/mach-msm/platsmp.h
@@ -0,0 +1,31 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * control for which core is the next to come out of the secondary
+ * boot "holding pen".
+ */
+extern volatile int pen_release;
+
+void __cpuinit msm_secondary_startup(void);
+void __cpuinit write_pen_release(int val);
+
+/* HOTPLUG Interface */
+int platform_cpu_kill(unsigned int cpu);
+void platform_cpu_die(unsigned int cpu);
+int platform_cpu_disable(unsigned int cpu);
+
+extern struct smp_operations arm_smp_ops __initdata;
+extern struct smp_operations msm8960_smp_ops __initdata;
+extern struct smp_operations msm8974_smp_ops __initdata;
+extern struct smp_operations msm8625_smp_ops __initdata;
+extern struct smp_operations scorpion_smp_ops __initdata;
diff --git a/arch/arm/mach-msm/pm-8x60.c b/arch/arm/mach-msm/pm-8x60.c
index 550bb56..b42ad94 100644
--- a/arch/arm/mach-msm/pm-8x60.c
+++ b/arch/arm/mach-msm/pm-8x60.c
@@ -558,15 +558,18 @@
 static bool msm_pm_power_collapse_standalone(bool from_idle)
 {
 	unsigned int cpu = smp_processor_id();
-	unsigned int avsdscr_setting;
-	unsigned int avscsr_enable;
+	unsigned int avsdscr;
+	unsigned int avscsr;
 	bool collapsed;
 
-	avsdscr_setting = avs_get_avsdscr();
-	avscsr_enable = avs_disable();
+	avsdscr = avs_get_avsdscr();
+	avscsr = avs_get_avscsr();
+	avs_set_avscsr(0); /* Disable AVS */
+
 	collapsed = msm_pm_spm_power_collapse(cpu, from_idle, false);
-	avs_enable(avscsr_enable);
-	avs_reset_delays(avsdscr_setting);
+
+	avs_set_avsdscr(avsdscr);
+	avs_set_avscsr(avscsr);
 	return collapsed;
 }
 
@@ -574,8 +577,8 @@
 {
 	unsigned int cpu = smp_processor_id();
 	unsigned long saved_acpuclk_rate;
-	unsigned int avsdscr_setting;
-	unsigned int avscsr_enable;
+	unsigned int avsdscr;
+	unsigned int avscsr;
 	bool collapsed;
 
 	if (MSM_PM_DEBUG_POWER_COLLAPSE & msm_pm_debug_mask)
@@ -586,8 +589,9 @@
 	if (MSM_PM_DEBUG_POWER_COLLAPSE & msm_pm_debug_mask)
 		pr_info("CPU%u: %s: pre power down\n", cpu, __func__);
 
-	avsdscr_setting = avs_get_avsdscr();
-	avscsr_enable = avs_disable();
+	avsdscr = avs_get_avsdscr();
+	avscsr = avs_get_avscsr();
+	avs_set_avscsr(0); /* Disable AVS */
 
 	if (cpu_online(cpu))
 		saved_acpuclk_rate = acpuclk_power_collapse();
@@ -629,8 +633,8 @@
 	}
 
 
-	avs_enable(avscsr_enable);
-	avs_reset_delays(avsdscr_setting);
+	avs_set_avsdscr(avsdscr);
+	avs_set_avscsr(avscsr);
 	msm_pm_config_hw_after_power_up();
 	if (MSM_PM_DEBUG_POWER_COLLAPSE & msm_pm_debug_mask)
 		pr_info("CPU%u: %s: post power up\n", cpu, __func__);
diff --git a/arch/arm/mach-msm/pm-boot.c b/arch/arm/mach-msm/pm-boot.c
index f32e149..53cc0f5 100644
--- a/arch/arm/mach-msm/pm-boot.c
+++ b/arch/arm/mach-msm/pm-boot.c
@@ -137,7 +137,7 @@
 			= msm_pm_config_rst_vector_after_pc;
 		break;
 	case MSM_PM_BOOT_CONFIG_REMAP_BOOT_ADDR:
-		if (!cpu_is_msm8625()) {
+		if (!cpu_is_msm8625() && !cpu_is_msm8625q()) {
 			void *remapped;
 
 			/*
@@ -200,7 +200,7 @@
 					pdata->v_addr + mpa5_cfg_ctl[0]);
 
 			/* 8x25Q changes */
-			if (SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 3) {
+			if (cpu_is_msm8625q()) {
 				/* write 'entry' to boot remapper register */
 				__raw_writel(entry, (pdata->v_addr +
 						mpa5_boot_remap_addr[1]));
diff --git a/arch/arm/mach-msm/pm2.c b/arch/arm/mach-msm/pm2.c
index 96c1218..ec9f030 100644
--- a/arch/arm/mach-msm/pm2.c
+++ b/arch/arm/mach-msm/pm2.c
@@ -562,7 +562,7 @@
 		__raw_writel(0, APPS_PWRDOWN);
 		mb();
 		msm_spm_reinit();
-	} else if (cpu_is_msm8625()) {
+	} else if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		__raw_writel(0, APPS_PWRDOWN);
 		mb();
 
@@ -881,7 +881,7 @@
 
 	memset(msm_pm_smem_data, 0, sizeof(*msm_pm_smem_data));
 
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		/* Program the SPM */
 		ret = msm_spm_set_low_power_mode(MSM_SPM_MODE_POWER_COLLAPSE,
 									false);
@@ -971,7 +971,7 @@
 #endif
 
 #ifdef CONFIG_CACHE_L2X0
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		l2cc_suspend();
 	else
 		apps_power_collapse = 1;
@@ -983,7 +983,7 @@
 	 * TBD: Currently recognise the MODEM early exit
 	 * path by reading the MPA5_GDFS_CNT_VAL register.
 	 */
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		int cpu;
 		/*
 		 * on system reset, default value of MPA5_GDFS_CNT_VAL
@@ -997,7 +997,7 @@
 		val = __raw_readl(MSM_CFG_CTL_BASE + 0x38);
 
 		/* 8x25Q */
-		if (SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 3) {
+		if (cpu_is_msm8625q()) {
 			if (val != 0x000F0002) {
 				for_each_possible_cpu(cpu) {
 					if (!cpu)
@@ -1031,7 +1031,7 @@
 	}
 
 #ifdef CONFIG_CACHE_L2X0
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		l2cc_resume();
 	else
 		apps_power_collapse = 0;
@@ -1153,7 +1153,7 @@
 
 	smd_sleep_exit();
 
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		ret = msm_spm_set_low_power_mode(MSM_SPM_MODE_CLOCK_GATING,
 									false);
 		WARN_ON(ret);
@@ -1220,7 +1220,7 @@
 		msm_cpr_ops->cpr_resume();
 
 power_collapse_bail:
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		ret = msm_spm_set_low_power_mode(MSM_SPM_MODE_CLOCK_GATING,
 									false);
 		WARN_ON(ret);
@@ -1258,14 +1258,14 @@
 #endif
 
 #ifdef CONFIG_CACHE_L2X0
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		l2cc_suspend();
 #endif
 
 	collapsed = msm_pm_collapse();
 
 #ifdef CONFIG_CACHE_L2X0
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		l2cc_resume();
 #endif
 
@@ -1310,7 +1310,7 @@
 			return -EIO;
 	}
 
-	if (!cpu_is_msm8625())
+	if (!cpu_is_msm8625() && !cpu_is_msm8625q())
 		msm_pm_config_hw_before_swfi();
 
 	msm_arch_idle();
@@ -1713,7 +1713,7 @@
 		return ret;
 	}
 
-	if (cpu_is_msm8625()) {
+	if (cpu_is_msm8625() || cpu_is_msm8625q()) {
 		target_type = TARGET_IS_8625;
 		clean_caches((unsigned long)&target_type, sizeof(target_type),
 				virt_to_phys(&target_type));
@@ -1725,7 +1725,7 @@
 		 * MPA5_GDFS_CNT_VAL[9:0] = Delay counter for
 		 * GDFS control.
 		 */
-		if (SOCINFO_VERSION_MAJOR(socinfo_get_version()) >= 3)
+		if (cpu_is_msm8625q())
 			val = 0x000F0002;
 		else
 			val = 0x00030002;
diff --git a/arch/arm/mach-msm/qdsp5/adsp.c b/arch/arm/mach-msm/qdsp5/adsp.c
index 6189da8..81af66b 100644
--- a/arch/arm/mach-msm/qdsp5/adsp.c
+++ b/arch/arm/mach-msm/qdsp5/adsp.c
@@ -1080,6 +1080,21 @@
 	return 0;
 }
 
+int msm_adsp_dump(struct msm_adsp_module *module)
+{
+	int rc = 0;
+	if (!module) {
+		MM_INFO("Invalid module. Dumps are not collected\n");
+		return -EINVAL;
+	}
+	MM_INFO("starting DSP DUMP\n");
+	rc = rpc_adsp_rtos_app_to_modem(RPC_ADSP_RTOS_CMD_CORE_DUMP,
+			module->id, module);
+	MM_INFO("DSP DUMP done rc =%d\n", rc);
+	return rc;
+}
+EXPORT_SYMBOL(msm_adsp_dump);
+
 int msm_adsp_enable(struct msm_adsp_module *module)
 {
 	int rc = 0;
@@ -1123,6 +1138,7 @@
 			rc = 0;
 		} else {
 			MM_ERR("module '%s' enable timed out\n", module->name);
+			msm_adsp_dump(module);
 			rc = -ETIMEDOUT;
 		}
 		if (module->open_count++ == 0 && module->clk)
diff --git a/arch/arm/mach-msm/qdsp5/adsp.h b/arch/arm/mach-msm/qdsp5/adsp.h
index 50f5b83..4e9d311 100644
--- a/arch/arm/mach-msm/qdsp5/adsp.h
+++ b/arch/arm/mach-msm/qdsp5/adsp.h
@@ -152,6 +152,7 @@
 	RPC_ADSP_RTOS_CMD_SET_STATE,
 	RPC_ADSP_RTOS_CMD_REMOTE_INIT_INFO_EVENT,
 	RPC_ADSP_RTOS_CMD_GET_INIT_INFO,
+	RPC_ADSP_RTOS_CMD_CORE_DUMP,
 };
 
 enum rpc_adsp_rtos_mod_status_type {
diff --git a/arch/arm/mach-msm/qdsp5/audio_aac.c b/arch/arm/mach-msm/qdsp5/audio_aac.c
index 46a80d7..c36cac7 100644
--- a/arch/arm/mach-msm/qdsp5/audio_aac.c
+++ b/arch/arm/mach-msm/qdsp5/audio_aac.c
@@ -261,8 +261,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -306,8 +308,12 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
+
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_ac3.c b/arch/arm/mach-msm/qdsp5/audio_ac3.c
index e453ec5..b5337bd 100644
--- a/arch/arm/mach-msm/qdsp5/audio_ac3.c
+++ b/arch/arm/mach-msm/qdsp5/audio_ac3.c
@@ -252,8 +252,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -296,8 +298,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_amrnb.c b/arch/arm/mach-msm/qdsp5/audio_amrnb.c
index 0792e3f..4aa7403 100644
--- a/arch/arm/mach-msm/qdsp5/audio_amrnb.c
+++ b/arch/arm/mach-msm/qdsp5/audio_amrnb.c
@@ -265,8 +265,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -310,8 +312,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_amrwb.c b/arch/arm/mach-msm/qdsp5/audio_amrwb.c
index 7d37cea..57df4ad 100644
--- a/arch/arm/mach-msm/qdsp5/audio_amrwb.c
+++ b/arch/arm/mach-msm/qdsp5/audio_amrwb.c
@@ -262,8 +262,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -307,8 +309,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_evrc.c b/arch/arm/mach-msm/qdsp5/audio_evrc.c
index 155b0e1..0799ee1 100644
--- a/arch/arm/mach-msm/qdsp5/audio_evrc.c
+++ b/arch/arm/mach-msm/qdsp5/audio_evrc.c
@@ -255,8 +255,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -299,8 +301,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_lpa.c b/arch/arm/mach-msm/qdsp5/audio_lpa.c
index 8120d7b..e896e85 100644
--- a/arch/arm/mach-msm/qdsp5/audio_lpa.c
+++ b/arch/arm/mach-msm/qdsp5/audio_lpa.c
@@ -293,9 +293,10 @@
 	cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 	rc = audmgr_enable(&audio->audmgr, &cfg);
-	if (rc < 0)
+	if (rc < 0) {
+		msm_adsp_dump(audio->audplay);
 		return rc;
-
+	}
 	if (msm_adsp_enable(audio->audplay)) {
 		MM_ERR("msm_adsp_enable(audplay) failed\n");
 		audmgr_disable(&audio->audmgr);
@@ -335,7 +336,9 @@
 		wake_up(&audio->write_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		audmgr_disable(&audio->audmgr);
+		rc = audmgr_disable(&audio->audmgr);
+		if (rc < 0)
+			msm_adsp_dump(audio->audplay);
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_mp3.c b/arch/arm/mach-msm/qdsp5/audio_mp3.c
index b8c64be..a606bd5 100644
--- a/arch/arm/mach-msm/qdsp5/audio_mp3.c
+++ b/arch/arm/mach-msm/qdsp5/audio_mp3.c
@@ -330,8 +330,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -376,8 +378,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_pcm.c b/arch/arm/mach-msm/qdsp5/audio_pcm.c
index 3eb72c8..d19f80b 100644
--- a/arch/arm/mach-msm/qdsp5/audio_pcm.c
+++ b/arch/arm/mach-msm/qdsp5/audio_pcm.c
@@ -299,9 +299,10 @@
 	cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 	rc = audmgr_enable(&audio->audmgr, &cfg);
-	if (rc < 0)
+	if (rc < 0) {
+		msm_adsp_dump(audio->audplay);
 		return rc;
-
+	}
 	if (msm_adsp_enable(audio->audplay)) {
 		MM_ERR("msm_adsp_enable(audplay) failed\n");
 		audmgr_disable(&audio->audmgr);
@@ -341,7 +342,9 @@
 		wake_up(&audio->write_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		audmgr_disable(&audio->audmgr);
+		rc = audmgr_disable(&audio->audmgr);
+		if (rc < 0)
+			msm_adsp_dump(audio->audplay);
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_pcm_in.c b/arch/arm/mach-msm/qdsp5/audio_pcm_in.c
index 68ffcfef..7b2090d 100644
--- a/arch/arm/mach-msm/qdsp5/audio_pcm_in.c
+++ b/arch/arm/mach-msm/qdsp5/audio_pcm_in.c
@@ -218,9 +218,10 @@
 	cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 	rc = audmgr_enable(&audio->audmgr, &cfg);
-	if (rc < 0)
+	if (rc < 0) {
+		msm_adsp_dump(audio->audrec);
 		return rc;
-
+	}
 	if (audpreproc_enable(audio->enc_id, &audpre_dsp_event, audio)) {
 		MM_ERR("msm_adsp_enable(audpreproc) failed\n");
 		audmgr_disable(&audio->audmgr);
@@ -249,6 +250,8 @@
 /* must be called with audio->lock held */
 static int audpcm_in_disable(struct audio_in *audio)
 {
+	int rc;
+
 	if (audio->enabled) {
 		audio->enabled = 0;
 
@@ -262,7 +265,9 @@
 		/*reset the sampling frequency information at audpreproc layer*/
 		audio->session_info.sampling_freq = 0;
 		audpreproc_update_audrec_info(&audio->session_info);
-		audmgr_disable(&audio->audmgr);
+		rc = audmgr_disable(&audio->audmgr);
+		if (rc < 0)
+			msm_adsp_dump(audio->audrec);
 	}
 	return 0;
 }
diff --git a/arch/arm/mach-msm/qdsp5/audio_qcelp.c b/arch/arm/mach-msm/qdsp5/audio_qcelp.c
index 876c909..3fc489c 100644
--- a/arch/arm/mach-msm/qdsp5/audio_qcelp.c
+++ b/arch/arm/mach-msm/qdsp5/audio_qcelp.c
@@ -251,8 +251,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -296,8 +298,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_wma.c b/arch/arm/mach-msm/qdsp5/audio_wma.c
index 6d520b4..f7d54cc 100644
--- a/arch/arm/mach-msm/qdsp5/audio_wma.c
+++ b/arch/arm/mach-msm/qdsp5/audio_wma.c
@@ -268,8 +268,10 @@
 		cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 		rc = audmgr_enable(&audio->audmgr, &cfg);
-		if (rc < 0)
+		if (rc < 0) {
+			msm_adsp_dump(audio->audplay);
 			return rc;
+		}
 	}
 
 	if (msm_adsp_enable(audio->audplay)) {
@@ -314,8 +316,11 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK)
-			audmgr_disable(&audio->audmgr);
+		if (audio->pcm_feedback == TUNNEL_MODE_PLAYBACK) {
+			rc = audmgr_disable(&audio->audmgr);
+			if (rc < 0)
+				msm_adsp_dump(audio->audplay);
+		}
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audio_wmapro.c b/arch/arm/mach-msm/qdsp5/audio_wmapro.c
index a08f3c9..8dba4a6 100644
--- a/arch/arm/mach-msm/qdsp5/audio_wmapro.c
+++ b/arch/arm/mach-msm/qdsp5/audio_wmapro.c
@@ -266,8 +266,10 @@
 	cfg.snd_method = RPC_SND_METHOD_MIDI;
 
 	rc = audmgr_enable(&audio->audmgr, &cfg);
-	if (rc < 0)
+	if (rc < 0) {
+		msm_adsp_dump(audio->audplay);
 		return rc;
+	}
 
 	if (msm_adsp_enable(audio->audplay)) {
 		MM_ERR("msm_adsp_enable(audplay) failed\n");
@@ -309,7 +311,10 @@
 		wake_up(&audio->read_wait);
 		msm_adsp_disable(audio->audplay);
 		audpp_disable(audio->dec_id, audio);
-		audmgr_disable(&audio->audmgr);
+		rc = audmgr_disable(&audio->audmgr);
+		if (rc < 0)
+			msm_adsp_dump(audio->audplay);
+
 		audio->out_needed = 0;
 		rmt_put_resource(audio);
 		audio->rmt_resource_released = 1;
diff --git a/arch/arm/mach-msm/qdsp5/audpp.c b/arch/arm/mach-msm/qdsp5/audpp.c
index b4ead5c..b4b7338f 100644
--- a/arch/arm/mach-msm/qdsp5/audpp.c
+++ b/arch/arm/mach-msm/qdsp5/audpp.c
@@ -292,6 +292,7 @@
 			MM_INFO("ENABLE\n");
 			if (!audpp->enabled) {
 				audpp->enabled = 1;
+				wake_up(&audpp->event_wait);
 				audpp_broadcast(audpp, id, msg);
 			} else {
 				cid = msg[1];
@@ -344,6 +345,7 @@
 	struct audpp_state *audpp = &the_audpp_state;
 	uint16_t msg[8];
 	int res = 0;
+	int rc;
 
 	if (id < -1 || id > 4)
 		return -EINVAL;
@@ -374,6 +376,11 @@
 		LOG(EV_ENABLE, 2);
 		msm_adsp_enable(audpp->mod);
 		audpp_dsp_config(1);
+		rc = wait_event_timeout(audpp->event_wait,
+					(audpp->enabled == 1),
+					3 * HZ);
+		if (rc == 0)
+			msm_adsp_dump(audpp->mod);
 	} else {
 		if (audpp->enabled) {
 			msg[0] = AUDPP_MSG_ENA_ENA;
@@ -424,13 +431,17 @@
 		MM_DBG("disable\n");
 		LOG(EV_DISABLE, 2);
 		audpp_dsp_config(0);
-		rc = wait_event_interruptible(audpp->event_wait,
-				(audpp->enabled == 0));
+		rc = wait_event_timeout(audpp->event_wait,
+					(audpp->enabled == 0),
+					3 * HZ);
 		if (audpp->enabled == 0)
 			MM_INFO("Received CFG_MSG_DISABLE from ADSP\n");
-		else
+		else {
 			MM_ERR("Didn't receive CFG_MSG DISABLE \
 					message from ADSP\n");
+			if (rc == 0)
+				msm_adsp_dump(audpp->mod);
+		}
 		msm_adsp_disable(audpp->mod);
 		msm_adsp_put(audpp->mod);
 		audpp->mod = NULL;
diff --git a/arch/arm/mach-msm/qdsp6v2/pcm_in_proxy.c b/arch/arm/mach-msm/qdsp6v2/pcm_in_proxy.c
index 84f136a..5faee21 100644
--- a/arch/arm/mach-msm/qdsp6v2/pcm_in_proxy.c
+++ b/arch/arm/mach-msm/qdsp6v2/pcm_in_proxy.c
@@ -72,31 +72,36 @@
 	if (pcm->start) {
 		if (pcm->dsp_idx == pcm->buffer_count)
 			pcm->dsp_idx = 0;
-		rc = wait_event_timeout(pcm->wait,
-				(pcm->dma_buf[pcm->dsp_idx].used == 0) ||
-				atomic_read(&pcm->in_stopped), 1 * HZ);
-		if (!rc) {
-			pr_err("%s: wait_event_timeout failed\n", __func__);
-			goto fail;
+		if (pcm->dma_buf[pcm->dsp_idx].used == 0) {
+			if (atomic_read(&pcm->in_stopped)) {
+				pr_err("%s: Driver closed - return\n",
+					__func__);
+				return HRTIMER_NORESTART;
+			}
+			rc = afe_rt_proxy_port_read(
+				pcm->dma_buf[pcm->dsp_idx].addr,
+				pcm->buffer_size);
+			if (rc < 0) {
+				pr_err("%s afe_rt_proxy_port_read fail\n",
+					__func__);
+				goto fail;
+			}
+			pcm->dma_buf[pcm->dsp_idx].used = 1;
+			pcm->dsp_idx++;
+			pr_debug("sending frame rec to DSP: poll_time: %d\n",
+					pcm->poll_time);
+		} else {
+			pr_err("Qcom: Used flag not reset retry after %d msec\n",
+				(pcm->poll_time/10));
+			goto fail_timer;
 		}
-		if (atomic_read(&pcm->in_stopped)) {
-			pr_err("%s: Driver closed - return\n", __func__);
-			return HRTIMER_NORESTART;
-		}
-		rc = afe_rt_proxy_port_read(
-			pcm->dma_buf[pcm->dsp_idx].addr,
-			pcm->buffer_size);
-		if (rc < 0) {
-			pr_err("%s afe_rt_proxy_port_read fail\n", __func__);
-			goto fail;
-		}
-		pcm->dma_buf[pcm->dsp_idx].used = 1;
-		pcm->dsp_idx++;
-		pr_debug("%s: sending frame rec to DSP: poll_time: %d\n",
-				__func__, pcm->poll_time);
 fail:
 		hrtimer_forward_now(hrt, ns_to_ktime(pcm->poll_time
 				* 1000));
+		return HRTIMER_RESTART;
+fail_timer:
+		hrtimer_forward_now(hrt, ns_to_ktime((pcm->poll_time/10)
+				* 1000));
 
 		return HRTIMER_RESTART;
 	} else {
diff --git a/arch/arm/mach-msm/ramdump.c b/arch/arm/mach-msm/ramdump.c
index e33ec48..aac49d0 100644
--- a/arch/arm/mach-msm/ramdump.c
+++ b/arch/arm/mach-msm/ramdump.c
@@ -11,14 +11,10 @@
  */
 
 #include <linux/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/reboot.h>
 #include <linux/workqueue.h>
 #include <linux/io.h>
 #include <linux/jiffies.h>
 #include <linux/sched.h>
-#include <linux/stringify.h>
-#include <linux/delay.h>
 #include <linux/module.h>
 #include <linux/miscdevice.h>
 #include <linux/fs.h>
@@ -26,8 +22,8 @@
 #include <linux/slab.h>
 #include <linux/poll.h>
 #include <linux/uaccess.h>
-
-#include <asm-generic/poll.h>
+#include <linux/elf.h>
+#include <linux/wait.h>
 
 #include "ramdump.h"
 
@@ -46,6 +42,8 @@
 	wait_queue_head_t dump_wait_q;
 	int nsegments;
 	struct ramdump_segment *segments;
+	size_t elfcore_size;
+	char *elfcore_buf;
 };
 
 static int ramdump_open(struct inode *inode, struct file *filep)
@@ -107,13 +105,29 @@
 	size_t copy_size = 0;
 	int ret = 0;
 
-	if (rd_dev->data_ready == 0) {
-		pr_err("Ramdump(%s): Read when there's no dump available!",
-			rd_dev->name);
-		return -EPIPE;
+	if ((filep->f_flags & O_NONBLOCK) && !rd_dev->data_ready)
+		return -EAGAIN;
+
+	ret = wait_event_interruptible(rd_dev->dump_wait_q, rd_dev->data_ready);
+	if (ret)
+		return ret;
+
+	if (*pos < rd_dev->elfcore_size) {
+		copy_size = min(rd_dev->elfcore_size, count);
+
+		if (copy_to_user(buf, rd_dev->elfcore_buf, copy_size)) {
+			ret = -EFAULT;
+			goto ramdump_done;
+		}
+		*pos += copy_size;
+		count -= copy_size;
+		buf += copy_size;
+		if (count == 0)
+			return copy_size;
 	}
 
-	addr = offset_translate(*pos, rd_dev, &data_left);
+	addr = offset_translate(*pos - rd_dev->elfcore_size, rd_dev,
+				&data_left);
 
 	/* EOF check */
 	if (data_left == 0) {
@@ -174,7 +188,7 @@
 	return mask;
 }
 
-const struct file_operations ramdump_file_ops = {
+static const struct file_operations ramdump_file_ops = {
 	.open = ramdump_open,
 	.release = ramdump_release,
 	.read = ramdump_read,
@@ -234,11 +248,14 @@
 	kfree(rd_dev);
 }
 
-int do_ramdump(void *handle, struct ramdump_segment *segments,
-		int nsegments)
+static int _do_ramdump(void *handle, struct ramdump_segment *segments,
+		int nsegments, bool use_elf)
 {
 	int ret, i;
 	struct ramdump_device *rd_dev = (struct ramdump_device *)handle;
+	Elf32_Phdr *phdr;
+	Elf32_Ehdr *ehdr;
+	unsigned long offset;
 
 	if (!rd_dev->consumer_present) {
 		pr_err("Ramdump(%s): No consumers. Aborting..\n", rd_dev->name);
@@ -251,6 +268,38 @@
 	rd_dev->segments = segments;
 	rd_dev->nsegments = nsegments;
 
+	if (use_elf) {
+		rd_dev->elfcore_size = sizeof(*ehdr) +
+				       sizeof(*phdr) * nsegments;
+		ehdr = kzalloc(rd_dev->elfcore_size, GFP_KERNEL);
+		rd_dev->elfcore_buf = (char *)ehdr;
+		if (!rd_dev->elfcore_buf)
+			return -ENOMEM;
+
+		memcpy(ehdr->e_ident, ELFMAG, SELFMAG);
+		ehdr->e_ident[EI_CLASS] = ELFCLASS32;
+		ehdr->e_ident[EI_DATA] = ELFDATA2LSB;
+		ehdr->e_ident[EI_VERSION] = EV_CURRENT;
+		ehdr->e_ident[EI_OSABI] = ELFOSABI_NONE;
+		ehdr->e_type = ET_CORE;
+		ehdr->e_version = EV_CURRENT;
+		ehdr->e_phoff = sizeof(*ehdr);
+		ehdr->e_ehsize = sizeof(*ehdr);
+		ehdr->e_phentsize = sizeof(*phdr);
+		ehdr->e_phnum = nsegments;
+
+		offset = rd_dev->elfcore_size;
+		phdr = (Elf32_Phdr *)(ehdr + 1);
+		for (i = 0; i < nsegments; i++, phdr++) {
+			phdr->p_type = PT_LOAD;
+			phdr->p_offset = offset;
+			phdr->p_vaddr = phdr->p_paddr = segments[i].address;
+			phdr->p_filesz = phdr->p_memsz = segments[i].size;
+			phdr->p_flags = PF_R | PF_W | PF_X;
+			offset += phdr->p_filesz;
+		}
+	}
+
 	rd_dev->data_ready = 1;
 	rd_dev->ramdump_status = -1;
 
@@ -271,5 +320,20 @@
 		ret = (rd_dev->ramdump_status == 0) ? 0 : -EPIPE;
 
 	rd_dev->data_ready = 0;
+	rd_dev->elfcore_size = 0;
+	kfree(rd_dev->elfcore_buf);
+	rd_dev->elfcore_buf = NULL;
 	return ret;
+
+}
+
+int do_ramdump(void *handle, struct ramdump_segment *segments, int nsegments)
+{
+	return _do_ramdump(handle, segments, nsegments, false);
+}
+
+int
+do_elf_ramdump(void *handle, struct ramdump_segment *segments, int nsegments)
+{
+	return _do_ramdump(handle, segments, nsegments, true);
 }
diff --git a/arch/arm/mach-msm/ramdump.h b/arch/arm/mach-msm/ramdump.h
index 3e5bfaf..5fb41ec 100644
--- a/arch/arm/mach-msm/ramdump.h
+++ b/arch/arm/mach-msm/ramdump.h
@@ -24,5 +24,7 @@
 void destroy_ramdump_device(void *dev);
 int do_ramdump(void *handle, struct ramdump_segment *segments,
 		int nsegments);
+int do_elf_ramdump(void *handle, struct ramdump_segment *segments,
+		int nsegments);
 
 #endif
diff --git a/arch/arm/mach-msm/rmt_storage_client.c b/arch/arm/mach-msm/rmt_storage_client.c
index 4bec55d..c4530a9 100644
--- a/arch/arm/mach-msm/rmt_storage_client.c
+++ b/arch/arm/mach-msm/rmt_storage_client.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2009-2011, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2009-2012, Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -28,6 +28,7 @@
 #include <linux/debugfs.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
+#include <linux/reboot.h>
 #include <asm/uaccess.h>
 #include <asm/pgtable.h>
 #include <mach/msm_rpcrouter.h>
@@ -116,6 +117,7 @@
 #endif
 
 static struct rmt_storage_client_info *rmc;
+struct rmt_storage_srv *rmt_srv;
 
 #ifdef CONFIG_MSM_SDIO_SMEM
 DECLARE_DELAYED_WORK(sdio_smem_work, rmt_storage_sdio_smem_work);
@@ -1348,6 +1350,61 @@
 			rmt_storage_get_sync_status(srv->rpc_client));
 }
 
+/*
+ * Initiate the remote storage force sync and wait until
+ * sync status is done or maximum 4 seconds in the reboot notifier.
+ * Usually RMT storage sync is not taking more than 2 seconds
+ * for encryption and sync.
+ */
+#define MAX_GET_SYNC_STATUS_TRIES 200
+#define GET_SYNC_STATUS_SLEEP_INTERVAL 20
+static int rmt_storage_reboot_call(
+	struct notifier_block *this, unsigned long code, void *cmd)
+{
+	int ret, count = 0;
+
+	switch (code) {
+	case SYS_RESTART:
+	case SYS_HALT:
+	case SYS_POWER_OFF:
+		pr_info("%s: Force RMT storage final sync...\n", __func__);
+		ret = rmt_storage_force_sync(rmt_srv->rpc_client);
+		if (ret)
+			break;
+
+		do {
+			count++;
+			msleep(GET_SYNC_STATUS_SLEEP_INTERVAL);
+			ret = rmt_storage_get_sync_status(rmt_srv->rpc_client);
+		} while (ret != 1 && count < MAX_GET_SYNC_STATUS_TRIES);
+
+		if (ret == 1)
+			pr_info("%s: RMT storage sync successful.\n", __func__);
+		else
+			pr_err("%s: RMT storage sync failed.\n", __func__);
+
+		pr_info("%s: Un register RMT storage client.\n", __func__);
+		msm_rpc_unregister_client(rmt_srv->rpc_client);
+		break;
+
+	default:
+		break;
+	}
+	return NOTIFY_DONE;
+}
+
+/*
+ * For the RMT storage sync, RPC channels are required. If we do not
+ * give max priority to RMT storage reboot notifier, RPC channels may get
+ * closed before RMT storage sync completed if RPC reboot notifier gets
+ * executed before this remotefs reboot notifier. Hence give the maximum
+ * priority to this reboot notifier.
+ */
+static struct notifier_block rmt_storage_reboot_notifier = {
+	.notifier_call = rmt_storage_reboot_call,
+	.priority = INT_MAX,
+};
+
 static int rmt_storage_init_ramfs(struct rmt_storage_srv *srv)
 {
 	struct shared_ramfs_table *ramfs_table;
@@ -1521,33 +1578,33 @@
 static int rmt_storage_probe(struct platform_device *pdev)
 {
 	struct rpcsvr_platform_device *dev;
-	struct rmt_storage_srv *srv;
 	int ret;
 
 	dev = container_of(pdev, struct rpcsvr_platform_device, base);
-	srv = rmt_storage_get_srv(dev->prog);
-	if (!srv) {
+	rmt_srv = rmt_storage_get_srv(dev->prog);
+
+	if (!rmt_srv) {
 		pr_err("%s: Invalid prog = %#x\n", __func__, dev->prog);
 		return -ENXIO;
 	}
 
-	rmt_storage_init_ramfs(srv);
-	rmt_storage_get_ramfs(srv);
+	rmt_storage_init_ramfs(rmt_srv);
+	rmt_storage_get_ramfs(rmt_srv);
 
-	INIT_DELAYED_WORK(&srv->restart_work, rmt_storage_restart_work);
+	INIT_DELAYED_WORK(&rmt_srv->restart_work, rmt_storage_restart_work);
 
 	/* Client Registration */
-	srv->rpc_client = msm_rpc_register_client2("rmt_storage",
+	rmt_srv->rpc_client = msm_rpc_register_client2("rmt_storage",
 						   dev->prog, dev->vers, 1,
 						   handle_rmt_storage_call);
-	if (IS_ERR(srv->rpc_client)) {
+	if (IS_ERR(rmt_srv->rpc_client)) {
 		pr_err("%s: Unable to register client (prog %.8x vers %.8x)\n",
 				__func__, dev->prog, dev->vers);
-		ret = PTR_ERR(srv->rpc_client);
+		ret = PTR_ERR(rmt_srv->rpc_client);
 		return ret;
 	}
 
-	ret = msm_rpc_register_reset_callbacks(srv->rpc_client,
+	ret = msm_rpc_register_reset_callbacks(rmt_srv->rpc_client,
 		handle_restart_teardown,
 		handle_restart_setup);
 	if (ret)
@@ -1557,12 +1614,18 @@
 		__func__, dev->prog);
 
 	/* register server callbacks */
-	ret = rmt_storage_reg_callbacks(srv->rpc_client);
+	ret = rmt_storage_reg_callbacks(rmt_srv->rpc_client);
 	if (ret)
 		goto unregister_client;
 
 	/* For targets that poll SMEM, set status to ready */
-	rmt_storage_set_client_status(srv, 1);
+	rmt_storage_set_client_status(rmt_srv, 1);
+
+	ret = register_reboot_notifier(&rmt_storage_reboot_notifier);
+	if (ret) {
+		pr_err("%s: Failed to register reboot notifier", __func__);
+		goto unregister_client;
+	}
 
 	ret = sysfs_create_group(&pdev->dev.kobj, &dev_attr_grp);
 	if (ret)
@@ -1571,7 +1634,7 @@
 	return 0;
 
 unregister_client:
-	msm_rpc_unregister_client(srv->rpc_client);
+	msm_rpc_unregister_client(rmt_srv->rpc_client);
 	return ret;
 }
 
diff --git a/arch/arm/mach-msm/rpc_server_handset.c b/arch/arm/mach-msm/rpc_server_handset.c
index 6d173fb..3a458c8 100644
--- a/arch/arm/mach-msm/rpc_server_handset.c
+++ b/arch/arm/mach-msm/rpc_server_handset.c
@@ -280,6 +280,13 @@
 	switch (key) {
 	case KEY_POWER:
 	case KEY_END:
+		if (hs->hs_pdata->ignore_end_key)
+			input_report_key(hs->ipdev, KEY_POWER,
+						(key_code != HS_REL_K));
+		else
+			input_report_key(hs->ipdev, key,
+						(key_code != HS_REL_K));
+		break;
 	case KEY_MEDIA:
 	case KEY_VOLUMEUP:
 	case KEY_VOLUMEDOWN:
diff --git a/arch/arm/mach-msm/sensors_adsp.c b/arch/arm/mach-msm/sensors_adsp.c
new file mode 100644
index 0000000..0683bc5
--- /dev/null
+++ b/arch/arm/mach-msm/sensors_adsp.c
@@ -0,0 +1,1249 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/workqueue.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/bitops.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/notifier.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/cdev.h>
+#include <linux/fs.h>
+#include <linux/platform_device.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/ctype.h>
+#include <linux/of_device.h>
+#include <linux/msm_dsps.h>
+#include <linux/uaccess.h>
+#include <asm/mach-types.h>
+#include <asm/arch_timer.h>
+#include <mach/subsystem_restart.h>
+#include <mach/ocmem.h>
+#include <mach/msm_smd.h>
+#include <mach/sensors_adsp.h>
+#include <mach/msm_bus.h>
+#include <mach/msm_bus_board.h>
+
+#define DRV_NAME	"sensors"
+#define DRV_VERSION	"1.00"
+
+#define SNS_OCMEM_SMD_CHANNEL	"SENSOR"
+#define SNS_OCMEM_CLIENT_ID     OCMEM_SENSORS
+#define SNS_OCMEM_SIZE          SZ_256K
+#define SMD_BUF_SIZE		2048
+#define SNS_TIMEOUT_MS    1000
+
+#define SNS_OCMEM_ALLOC_GROW    0x00000001
+#define SNS_OCMEM_ALLOC_SHRINK  0x00000002
+#define SNS_OCMEM_MAP_DONE      0x00000004
+#define SNS_OCMEM_MAP_FAIL      0x00000008
+#define SNS_OCMEM_UNMAP_DONE    0x00000010
+#define SNS_OCMEM_UNMAP_FAIL    0x00000020
+
+#define DSPS_HAS_CLIENT         0x00000100
+#define DSPS_HAS_NO_CLIENT      0x00000200
+#define DSPS_BW_VOTE_ON         0x00000400
+#define DSPS_BW_VOTE_OFF        0x00000800
+#define DSPS_PHYS_ADDR_SET      0x00001000
+
+/**
+ *  Structure contains all state used by the sensors driver
+ */
+struct sns_adsp_control_s {
+	wait_queue_head_t sns_wait;
+	spinlock_t sns_lock;
+	struct workqueue_struct *sns_workqueue;
+	struct work_struct sns_work;
+	smd_channel_t *smd_ch;
+	uint32_t sns_ocmem_status;
+	uint32_t mem_segments_size;
+	struct sns_mem_segment_s_v01 mem_segments[SNS_OCMEM_MAX_NUM_SEG_V01];
+	struct ocmem_buf *buf;
+	struct ocmem_map_list map_list;
+	struct ocmem_notifier *ocmem_handle;
+	bool ocmem_enabled;
+	struct notifier_block ocmem_nb;
+	uint32_t sns_ocmem_bus_client;
+	struct platform_device *pdev;
+	void *pil;
+	struct class *dev_class;
+	dev_t dev_num;
+	struct device *dev;
+	struct cdev *cdev;
+};
+
+static struct sns_adsp_control_s sns_ctl;
+
+/* All asynchronous responses from the OCMEM driver are received
+by this function */
+int sns_ocmem_drv_cb(struct notifier_block *self,
+			unsigned long action,
+			void *dev)
+{
+	unsigned long flags;
+	pr_debug("%s\n", __func__);
+
+	spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+
+	pr_debug("%s: Received OCMEM callback: action=%li\n",
+		__func__, action);
+
+	switch (action) {
+	case OCMEM_MAP_DONE:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_MAP_DONE;
+		sns_ctl.sns_ocmem_status &= (~OCMEM_MAP_FAIL &
+						~SNS_OCMEM_UNMAP_DONE &
+						~SNS_OCMEM_UNMAP_FAIL);
+		break;
+	case OCMEM_MAP_FAIL:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_MAP_FAIL;
+		sns_ctl.sns_ocmem_status &= (~OCMEM_MAP_DONE &
+						~SNS_OCMEM_UNMAP_DONE &
+						~SNS_OCMEM_UNMAP_FAIL);
+		break;
+	case OCMEM_UNMAP_DONE:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_UNMAP_DONE;
+		sns_ctl.sns_ocmem_status &= (~SNS_OCMEM_UNMAP_FAIL &
+						~SNS_OCMEM_MAP_DONE &
+						~OCMEM_MAP_FAIL);
+		break;
+	case OCMEM_UNMAP_FAIL:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_UNMAP_FAIL;
+		sns_ctl.sns_ocmem_status &= (~SNS_OCMEM_UNMAP_DONE &
+						~SNS_OCMEM_MAP_DONE &
+						~OCMEM_MAP_FAIL);
+		break;
+	case OCMEM_ALLOC_GROW:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_ALLOC_GROW;
+		sns_ctl.sns_ocmem_status &= ~SNS_OCMEM_ALLOC_SHRINK;
+		break;
+	case OCMEM_ALLOC_SHRINK:
+		sns_ctl.sns_ocmem_status |= SNS_OCMEM_ALLOC_SHRINK;
+		sns_ctl.sns_ocmem_status &= ~SNS_OCMEM_ALLOC_GROW;
+		break;
+	default:
+		pr_err("%s: Unknown action received in OCMEM callback %lu\n",
+						__func__, action);
+		break;
+	}
+
+	pr_debug("%s: sns_ocmem_status: 0x%x\n", __func__,
+					sns_ctl.sns_ocmem_status);
+	spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+
+	wake_up(&sns_ctl.sns_wait);
+
+	return 0;
+}
+
+/**
+ * Processes messages received through SMD from the ADSP
+ *
+ * @param hdr The message header
+ * @param msg Message pointer
+ *
+ * */
+void sns_ocmem_smd_process(struct sns_ocmem_hdr_s *hdr, void *msg)
+{
+	unsigned long flags;
+	pr_debug("%s\n", __func__);
+
+	spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+
+	pr_debug("%s: Received message from ADSP; id: %i type: %i (%08x)\n",
+		__func__, hdr->msg_id, hdr->msg_type,
+		sns_ctl.sns_ocmem_status);
+
+	if (hdr->msg_id == SNS_OCMEM_PHYS_ADDR_RESP_V01 &&
+	    hdr->msg_type == SNS_OCMEM_MSG_TYPE_RESP) {
+		struct sns_ocmem_phys_addr_resp_msg_v01 *msg_ptr =
+				(struct sns_ocmem_phys_addr_resp_msg_v01 *)msg;
+		pr_debug("%s: Received SNS_OCMEM_PHYS_ADDR_RESP_V01\n",
+			__func__);
+		pr_debug("%s: segments_valid=%d, segments_len=%d\n", __func__,
+				msg_ptr->segments_valid, msg_ptr->segments_len);
+
+		if (msg_ptr->segments_valid) {
+			sns_ctl.mem_segments_size = msg_ptr->segments_len;
+			memcpy(sns_ctl.mem_segments, msg_ptr->segments,
+				sizeof(struct sns_mem_segment_s_v01) *
+				msg_ptr->segments_len);
+
+			sns_ctl.sns_ocmem_status |= DSPS_PHYS_ADDR_SET;
+		} else {
+			pr_err("%s: Received invalid segment list\n", __func__);
+		}
+	} else if (hdr->msg_id == SNS_OCMEM_HAS_CLIENT_IND_V01  &&
+		   hdr->msg_type == SNS_OCMEM_MSG_TYPE_IND) {
+		struct sns_ocmem_has_client_ind_msg_v01 *msg_ptr =
+				(struct sns_ocmem_has_client_ind_msg_v01 *)msg;
+
+		pr_debug("%s: Received SNS_OCMEM_HAS_CLIENT_IND_V01\n",
+			__func__);
+		pr_debug("%s: ADSP has %i client(s)\n", __func__,
+			msg_ptr->num_clients);
+		if (msg_ptr->num_clients > 0) {
+			sns_ctl.sns_ocmem_status |= DSPS_HAS_CLIENT;
+			sns_ctl.sns_ocmem_status &= ~DSPS_HAS_NO_CLIENT;
+		} else {
+			sns_ctl.sns_ocmem_status |= DSPS_HAS_NO_CLIENT;
+			sns_ctl.sns_ocmem_status &= ~DSPS_HAS_CLIENT;
+		}
+	} else if (hdr->msg_id == SNS_OCMEM_BW_VOTE_RESP_V01 &&
+		   hdr->msg_type == SNS_OCMEM_MSG_TYPE_RESP) {
+		/* no need to handle this response msg, just return */
+		pr_debug("%s: Received SNS_OCMEM_BW_VOTE_RESP_V01\n", __func__);
+		spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+		return;
+	} else if (hdr->msg_id == SNS_OCMEM_BW_VOTE_IND_V01 &&
+		   hdr->msg_type == SNS_OCMEM_MSG_TYPE_IND) {
+		struct sns_ocmem_bw_vote_ind_msg_v01 *msg_ptr =
+			(struct sns_ocmem_bw_vote_ind_msg_v01 *)msg;
+		pr_debug("%s: Received BW_VOTE_IND_V01, is_vote_on=%d\n",
+						__func__, msg_ptr->is_vote_on);
+
+		if (msg_ptr->is_vote_on) {
+			sns_ctl.sns_ocmem_status |= DSPS_BW_VOTE_ON;
+			sns_ctl.sns_ocmem_status &= ~DSPS_BW_VOTE_OFF;
+		} else {
+			sns_ctl.sns_ocmem_status |= DSPS_BW_VOTE_OFF;
+			sns_ctl.sns_ocmem_status &= ~DSPS_BW_VOTE_ON;
+		}
+	} else {
+		pr_err("%s: Unknown message type received. id: %i; type: %i\n",
+					__func__, hdr->msg_id, hdr->msg_type);
+	}
+
+	pr_debug("%s: sns_ocmem_status: 0x%x\n",
+		__func__, sns_ctl.sns_ocmem_status);
+
+	spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+
+	wake_up(&sns_ctl.sns_wait);
+}
+
+/**
+ * All SMD notifications and messages from Sensors on ADSP are
+ * received by this function
+ *
+ * */
+
+void sns_ocmem_smd_notify_data(void *data, unsigned int event)
+{
+	pr_debug("%s:\n", __func__);
+
+	if (event == SMD_EVENT_DATA) {
+		int len;
+		pr_debug("%s: Received SMD event Data\n", __func__);
+		len = smd_read_avail(sns_ctl.smd_ch);
+		pr_debug("%s: len=%d\n", __func__, len);
+		if (len > 0) {
+			data = kzalloc(SMD_BUF_SIZE, GFP_ATOMIC);
+			if (data == NULL) {
+				pr_err("%s: malloc failed", __func__);
+				return;
+			}
+
+			len = smd_read_from_cb(sns_ctl.smd_ch,
+						data, SMD_BUF_SIZE);
+			if (len > 0) {
+				sns_ocmem_smd_process(
+					(struct sns_ocmem_hdr_s *) data,
+					(void *)((char *)data +
+					sizeof(struct sns_ocmem_hdr_s)));
+			} else {
+				pr_err("Failed to read event from smd %i", len);
+			}
+			kfree(data);
+		} else if (len < 0) {
+			pr_err("Failed to read event from smd %i", len);
+		}
+	} else if (event == SMD_EVENT_OPEN) {
+		pr_debug("%s: Received SMD event Open\n", __func__);
+	} else if (event == SMD_EVENT_CLOSE) {
+		pr_debug("%s: Received SMD event Close\n", __func__);
+	}
+}
+
+static bool sns_ocmem_is_status_set(uint32_t sns_ocmem_status)
+{
+	unsigned long flags;
+	bool is_set;
+	pr_debug("%s: status=0x%x\n", __func__, sns_ocmem_status);
+
+	spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+	is_set = sns_ctl.sns_ocmem_status & sns_ocmem_status;
+	spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+	pr_debug("%s: is_set=%d\n", __func__, is_set);
+	return is_set;
+}
+
+/**
+ * Wait for a response from ADSP or OCMEM Driver, timeout if necessary
+ *
+ * @param sns_ocmem_status Status flags to wait for.
+ * @param timeout_sec Seconds to wait before timeout
+ * @param timeout_nsec Nanoseconds to wait.  Total timeout = nsec + sec
+ *
+ * @return 0 If any status flag is set at any time prior to a timeout.
+ *	0 if success or timedout ; <0 for failures
+ *
+ */
+static int sns_ocmem_wait(uint32_t sns_ocmem_status,
+			  uint32_t timeout_ms)
+{
+	int err;
+	pr_debug("%s: status=0x%x, timeout_ms=%d\n", __func__,
+						sns_ocmem_status, timeout_ms);
+	if (timeout_ms) {
+		err = wait_event_interruptible_timeout(sns_ctl.sns_wait,
+			sns_ocmem_is_status_set(sns_ocmem_status),
+			msecs_to_jiffies(timeout_ms));
+
+		if (err == 0)
+			pr_err("%s: interruptible_timeout timeout err=%i\n",
+							__func__, err);
+		else if (err < 0)
+			pr_err("%s: interruptible_timeout failed err=%i\n",
+							__func__, err);
+	} else { /* no timeout */
+		err = wait_event_interruptible(sns_ctl.sns_wait,
+			sns_ocmem_is_status_set(sns_ocmem_status));
+		if (err < 0)
+			pr_err("%s: wait_event_interruptible failed err=%i\n",
+						__func__, err);
+	}
+
+	return err;
+}
+
+/**
+ * Sends a message to the ADSP via SMD.
+ *
+ * @param hdr Specifies message type and other meta data
+ * @param msg_ptr Pointer to the message contents.
+ *                Must be freed within this function if no error is returned.
+ *
+ * @return 0 upon success; < 0 upon error
+ */
+static int
+sns_ocmem_send_msg(struct sns_ocmem_hdr_s *hdr, void const *msg_ptr)
+{
+	int rv = 0;
+	int err = 0;
+	void *temp = NULL;
+	int size = sizeof(struct sns_ocmem_hdr_s) + hdr->msg_size;
+
+	temp = kzalloc(sizeof(struct sns_ocmem_hdr_s) + hdr->msg_size,
+			GFP_KERNEL);
+	pr_debug("%s size=%d\n", __func__, size);
+
+	if (temp == NULL) {
+		pr_err("%s: allocation failure\n", __func__);
+		rv = -ENOMEM;
+	}
+
+	hdr->dst_module = SNS_OCMEM_MODULE_ADSP;
+	hdr->src_module = SNS_OCMEM_MODULE_KERNEL;
+
+	memcpy(temp, hdr, sizeof(struct sns_ocmem_hdr_s));
+	memcpy((char *)temp + sizeof(struct sns_ocmem_hdr_s),
+		msg_ptr, hdr->msg_size);
+	pr_debug("%s: send msg type: %i size: %i id: %i dst: %i src: %i\n",
+				__func__, hdr->msg_type, hdr->msg_size,
+				hdr->msg_id, hdr->dst_module, hdr->src_module);
+
+	if (hdr == NULL) {
+		pr_err("%s: NULL message header\n", __func__);
+		rv = -EINVAL;
+	} else {
+		if (sns_ctl.smd_ch == NULL) {
+			pr_err("%s: null smd_ch\n", __func__);
+			rv = -EINVAL;
+		}
+		err = smd_write(sns_ctl.smd_ch, temp, size);
+		if (err < 0) {
+			pr_err("%s: smd_write failed %i\n", __func__, err);
+			rv = -ECOMM;
+		} else {
+			pr_debug("%s smd_write successful ret=%d\n",
+				__func__, err);
+		}
+	}
+
+	kfree(temp);
+
+	return rv;
+}
+
+/**
+ *  Load ADSP Firmware.
+ */
+
+static int sns_load_adsp(void)
+{
+	pr_debug("%s.\n", __func__);
+
+	sns_ctl.pil = subsystem_get("adsp");
+	if (IS_ERR(sns_ctl.pil)) {
+		pr_err("%s: fail to load ADSP firmware\n", __func__);
+		return -ENODEV;
+	}
+
+	pr_debug("%s: Q6/ADSP image is loaded\n", __func__);
+
+	return 0;
+}
+
+static int sns_ocmem_platform_data_populate(struct platform_device *pdev)
+{
+	int ret;
+	struct msm_bus_scale_pdata *sns_ocmem_bus_scale_pdata = NULL;
+	struct msm_bus_vectors *sns_ocmem_bus_vectors = NULL;
+	struct msm_bus_paths *ocmem_sns_bus_paths = NULL;
+	u32 val;
+
+	if (!pdev->dev.of_node) {
+		pr_err("%s: device tree information missing\n", __func__);
+		return -ENODEV;
+	}
+
+	sns_ocmem_bus_vectors = kzalloc(sizeof(struct msm_bus_vectors),
+					GFP_KERNEL);
+	if (!sns_ocmem_bus_vectors) {
+		dev_err(&pdev->dev, "Failed to allocate memory for platform data\n");
+		return -ENOMEM;
+	}
+
+	ret = of_property_read_u32(pdev->dev.of_node,
+				"qcom,src-id", &val);
+	if (ret) {
+		dev_err(&pdev->dev, "%s: qcom,src-id missing in DT node\n",
+				__func__);
+		goto fail1;
+	}
+	sns_ocmem_bus_vectors->src = val;
+	ret = of_property_read_u32(pdev->dev.of_node,
+				"qcom,dst-id", &val);
+	if (ret) {
+		dev_err(&pdev->dev, "%s: qcom,dst-id missing in DT node\n",
+				__func__);
+		goto fail1;
+	}
+	sns_ocmem_bus_vectors->dst = val;
+	ret = of_property_read_u32(pdev->dev.of_node,
+				"qcom,ab", &val);
+	if (ret) {
+		dev_err(&pdev->dev, "%s: qcom,ab missing in DT node\n",
+					__func__);
+		goto fail1;
+	}
+	sns_ocmem_bus_vectors->ab = val;
+	ret = of_property_read_u32(pdev->dev.of_node,
+				"qcom,ib", &val);
+	if (ret) {
+		dev_err(&pdev->dev, "%s: qcom,ib missing in DT node\n",
+					__func__);
+		goto fail1;
+	}
+	sns_ocmem_bus_vectors->ib = val;
+	ocmem_sns_bus_paths = kzalloc(sizeof(struct msm_bus_paths),
+					GFP_KERNEL);
+
+	if (!ocmem_sns_bus_paths) {
+		dev_err(&pdev->dev, "Failed to allocate memory for platform data\n");
+		goto fail1;
+	}
+	ocmem_sns_bus_paths->num_paths = 1;
+	ocmem_sns_bus_paths->vectors = sns_ocmem_bus_vectors;
+
+	sns_ocmem_bus_scale_pdata =
+			kzalloc(sizeof(struct msm_bus_scale_pdata), GFP_KERNEL);
+	if (!sns_ocmem_bus_scale_pdata) {
+		dev_err(&pdev->dev, "Failed to allocate memory for platform data\n");
+		goto fail2;
+	}
+
+	sns_ocmem_bus_scale_pdata->usecase = ocmem_sns_bus_paths;
+	sns_ocmem_bus_scale_pdata->num_usecases = 1;
+	sns_ocmem_bus_scale_pdata->name = "sensors-ocmem";
+
+	dev_set_drvdata(&pdev->dev, sns_ocmem_bus_scale_pdata);
+	return ret;
+
+fail2:
+	kfree(ocmem_sns_bus_paths);
+fail1:
+	kfree(sns_ocmem_bus_vectors);
+	return ret;
+}
+
+
+/**
+ * Initialize all sensors ocmem driver data fields and register with the
+ * ocmem driver.
+ *
+ * @return 0 upon success; < 0 upon error
+ */
+static int sns_ocmem_init(void)
+{
+	int i, err, ret;
+	struct sns_ocmem_hdr_s addr_req_hdr;
+	struct msm_bus_scale_pdata *sns_ocmem_bus_scale_pdata = NULL;
+
+	pr_debug("%s\n", __func__);
+
+	/* register from OCMEM callack */
+	sns_ctl.ocmem_handle =
+		ocmem_notifier_register(SNS_OCMEM_CLIENT_ID,
+		&sns_ctl.ocmem_nb);
+	if (sns_ctl.ocmem_handle == NULL) {
+		pr_err("OCMEM notifier registration failed\n");
+		return -EFAULT;
+	}
+
+	/* populate platform data */
+	ret = sns_ocmem_platform_data_populate(sns_ctl.pdev);
+	if (ret) {
+		dev_err(&sns_ctl.pdev->dev,
+			"%s: failed to populate platform data, rc = %d\n",
+			__func__, ret);
+		return -ENODEV;
+	}
+	sns_ocmem_bus_scale_pdata = dev_get_drvdata(&sns_ctl.pdev->dev);
+
+	sns_ctl.sns_ocmem_bus_client =
+		msm_bus_scale_register_client(sns_ocmem_bus_scale_pdata);
+
+	if (!sns_ctl.sns_ocmem_bus_client) {
+		pr_err("%s: msm_bus_scale_register_client() failed\n",
+			__func__);
+		return -EFAULT;
+	}
+
+	/* load ADSP first */
+	if (sns_load_adsp() != 0) {
+		pr_err("%s: sns_load_adsp failed\n", __func__);
+		return -EFAULT;
+	}
+
+	/* wait before open SMD channel from kernel to ensure
+	channel has been openned already from ADSP side */
+	pr_debug("%s: sleep for 1000 ms\n", __func__);
+	msleep(1000);
+
+	err = smd_named_open_on_edge(SNS_OCMEM_SMD_CHANNEL,
+					SMD_APPS_QDSP,
+					&sns_ctl.smd_ch,
+					NULL,
+					sns_ocmem_smd_notify_data);
+	if (err != 0) {
+		pr_err("%s: smd_named_open_on_edge failed %i\n", __func__, err);
+		return -EFAULT;
+	}
+
+	pr_debug("%s: SMD channel openned successfuly!\n", __func__);
+	/* wait for the channel ready before writing data */
+	pr_debug("%s: sleep for 1000 ms\n", __func__);
+	msleep(1000);
+	pr_debug("%s sending PHYS_ADDR_REQ\n", __func__);
+	addr_req_hdr.msg_id = SNS_OCMEM_PHYS_ADDR_REQ_V01;
+	addr_req_hdr.msg_type = SNS_OCMEM_MSG_TYPE_REQ;
+	addr_req_hdr.msg_size = 0;
+
+	err = sns_ocmem_send_msg(&addr_req_hdr, NULL);
+	if (err != 0) {
+		pr_err("%s: sns_ocmem_send_msg failed %i\n", __func__, err);
+		return -ECOMM;
+	}
+
+	err = sns_ocmem_wait(DSPS_PHYS_ADDR_SET, 0);
+	if (err != 0) {
+		pr_err("%s: sns_ocmem_wait failed %i\n", __func__, err);
+		return -EFAULT;
+	}
+
+	sns_ctl.map_list.num_chunks = sns_ctl.mem_segments_size;
+	for (i = 0; i < sns_ctl.mem_segments_size; i++) {
+		sns_ctl.map_list.chunks[i].ro =
+			sns_ctl.mem_segments[i].type == 1 ? true : false;
+		sns_ctl.map_list.chunks[i].ddr_paddr =
+			sns_ctl.mem_segments[i].start_address;
+		sns_ctl.map_list.chunks[i].size =
+			sns_ctl.mem_segments[i].size;
+
+		pr_debug("%s: chunks[%d]: ro=%d, ddr_paddr=0x%lx, size=%li",
+				__func__, i,
+				sns_ctl.map_list.chunks[i].ro,
+				sns_ctl.map_list.chunks[i].ddr_paddr,
+				sns_ctl.map_list.chunks[i].size);
+	}
+
+	return 0;
+}
+
+/**
+ *  Unmaps memory in ocmem back to DDR, indicates to the ADSP its completion,
+ *  and waits for it to finish removing its bandwidth vote.
+ *
+ */
+static void sns_ocmem_unmap(void)
+{
+	unsigned long flags;
+	int err = 0;
+	pr_debug("%s\n", __func__);
+
+	ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+				sns_ctl.buf, OCMEM_ON);
+
+	spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+	sns_ctl.sns_ocmem_status &= (~SNS_OCMEM_UNMAP_FAIL &
+					~SNS_OCMEM_UNMAP_DONE);
+	spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+
+	err = ocmem_unmap(SNS_OCMEM_CLIENT_ID,
+				sns_ctl.buf,
+				&sns_ctl.map_list);
+
+	if (err != 0) {
+		pr_err("ocmem_unmap failed %i\n", err);
+	} else {
+		err = sns_ocmem_wait(SNS_OCMEM_UNMAP_DONE |
+					SNS_OCMEM_UNMAP_FAIL, 0);
+
+		if (err == 0) {
+			if (sns_ocmem_is_status_set(SNS_OCMEM_UNMAP_DONE))
+				pr_debug("%s: OCMEM_UNMAP_DONE\n", __func__);
+			else if (sns_ocmem_is_status_set(
+							SNS_OCMEM_UNMAP_FAIL)) {
+				pr_err("%s: OCMEM_UNMAP_FAIL\n", __func__);
+				BUG_ON(true);
+			} else
+				pr_err("%s: status flag not set\n", __func__);
+		} else {
+			pr_err("%s: sns_ocmem_wait failed %i\n",
+					__func__, err);
+		}
+	}
+
+	ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+				sns_ctl.buf, OCMEM_OFF);
+}
+
+/**
+ * Waits for allocation to succeed.  This may take considerable time if the device
+ * is presently in a high-power use case.
+ *
+ * @return 0 on success; < 0 upon error
+ */
+static int sns_ocmem_wait_for_alloc(void)
+{
+	int err = 0;
+	pr_debug("%s\n", __func__);
+
+	err = sns_ocmem_wait(SNS_OCMEM_ALLOC_GROW |
+				DSPS_HAS_NO_CLIENT, 0);
+
+	if (err == 0) {
+		if (sns_ocmem_is_status_set(DSPS_HAS_NO_CLIENT)) {
+			pr_debug("%s: Lost client while waiting for GROW\n",
+				__func__);
+			ocmem_free(SNS_OCMEM_CLIENT_ID, sns_ctl.buf);
+			sns_ctl.buf = NULL;
+			return -EPIPE;
+		}
+	} else {
+		pr_err("sns_ocmem_wait failed %i\n", err);
+		return -EFAULT;
+	}
+
+	return 0;
+}
+
+/**
+ * Kicks-off the mapping of memory from DDR to ocmem.  Waits for the process
+ * to complete, then indicates so to the ADSP.
+ *
+ * @return 0: Success; < 0: Other error
+ */
+static int sns_ocmem_map(void)
+{
+	int err = 0;
+	unsigned long flags;
+	pr_debug("%s\n", __func__);
+
+	spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+	sns_ctl.sns_ocmem_status &=
+			(~SNS_OCMEM_MAP_FAIL & ~SNS_OCMEM_MAP_DONE);
+	spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+
+	/* vote for ocmem bus bandwidth */
+	err = msm_bus_scale_client_update_request(
+				sns_ctl.sns_ocmem_bus_client,
+				0);
+	if (err)
+		pr_err("%s: failed to vote for bus bandwidth\n", __func__);
+
+	err = ocmem_map(SNS_OCMEM_CLIENT_ID,
+			sns_ctl.buf,
+			&sns_ctl.map_list);
+
+	if (err != 0) {
+		pr_debug("ocmem_map failed %i\n", err);
+		ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+					sns_ctl.buf, OCMEM_OFF);
+		ocmem_free(SNS_OCMEM_CLIENT_ID, sns_ctl.buf);
+		sns_ctl.buf = NULL;
+	} else {
+		err = sns_ocmem_wait(SNS_OCMEM_ALLOC_SHRINK |
+					DSPS_HAS_NO_CLIENT |
+					SNS_OCMEM_MAP_DONE |
+					SNS_OCMEM_MAP_FAIL, 0);
+
+		if (err == 0) {
+			if (sns_ocmem_is_status_set(SNS_OCMEM_MAP_DONE))
+				pr_debug("%s: OCMEM mapping DONE\n", __func__);
+			else if (sns_ocmem_is_status_set(DSPS_HAS_NO_CLIENT)) {
+				pr_debug("%s: Lost client while waiting for MAP\n",
+					__func__);
+				sns_ocmem_unmap();
+				ocmem_free(SNS_OCMEM_CLIENT_ID,
+						sns_ctl.buf);
+				sns_ctl.buf = NULL;
+				err = -EPIPE;
+			} else if (sns_ocmem_is_status_set(
+						SNS_OCMEM_ALLOC_SHRINK)) {
+				pr_debug("%s: SHRINK while wait for MAP\n",
+					__func__);
+				sns_ocmem_unmap();
+				err = ocmem_shrink(SNS_OCMEM_CLIENT_ID,
+						sns_ctl.buf, 0);
+				BUG_ON(err != 0);
+				err = -EFAULT;
+			} else if (sns_ocmem_is_status_set(
+						SNS_OCMEM_MAP_FAIL)) {
+				pr_err("%s: OCMEM mapping fails\n", __func__);
+				ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+							sns_ctl.buf,
+							OCMEM_OFF);
+				ocmem_free(SNS_OCMEM_CLIENT_ID,
+						sns_ctl.buf);
+				sns_ctl.buf = NULL;
+			} else
+				pr_err("%s: status flag not set\n", __func__);
+		} else {
+			pr_err("sns_ocmem_wait failed %i\n", err);
+		}
+	}
+
+	return err;
+}
+
+/**
+ * Allocates memory in ocmem and maps to it from DDR.
+ *
+ * @return 0 upon success; <0 upon failure;
+ */
+static int sns_ocmem_alloc(void)
+{
+	int err = 0;
+	unsigned long flags;
+	pr_debug("%s\n", __func__);
+
+	if (sns_ctl.buf == NULL) {
+		spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+		sns_ctl.sns_ocmem_status &= ~SNS_OCMEM_ALLOC_GROW &
+						~SNS_OCMEM_ALLOC_SHRINK;
+		spin_unlock_irqrestore(&sns_ctl.sns_lock, flags);
+		sns_ctl.buf = ocmem_allocate_nb(SNS_OCMEM_CLIENT_ID,
+						SNS_OCMEM_SIZE);
+
+		if (sns_ctl.buf == NULL) {
+			pr_err("ocmem_allocate_nb returned NULL\n");
+			sns_ctl.ocmem_enabled = false;
+			err = -EFAULT;
+		} else if (sns_ctl.buf->len != 0 &&
+			SNS_OCMEM_SIZE > sns_ctl.buf->len) {
+			pr_err("ocmem_allocate_nb: invalid len %li, Req: %i)\n",
+				sns_ctl.buf->len, SNS_OCMEM_SIZE);
+			sns_ctl.ocmem_enabled = false;
+			err = -EFAULT;
+		}
+	}
+
+	pr_debug("%s OCMEM buf=%lx, buffer len=%li\n", __func__,
+			sns_ctl.buf->addr, sns_ctl.buf->len);
+
+	while (sns_ctl.ocmem_enabled) {
+		if (sns_ctl.buf->len == 0) {
+			pr_debug("%s: Waiting for memory allocation\n",
+				__func__);
+			err = sns_ocmem_wait_for_alloc();
+			if (err == -EPIPE) {
+				pr_debug("%s:Lost client while wait for alloc\n",
+					__func__);
+				break;
+			} else if (err != 0) {
+				pr_err("sns_ocmem_wait_for_alloc failed %i\n",
+					err);
+				break;
+			}
+		}
+
+		ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+					sns_ctl.buf,
+					OCMEM_ON);
+
+		err = sns_ocmem_map();
+
+		if (err == -EPIPE) {
+			pr_debug("%s: Lost client while waiting for mapping\n",
+				__func__);
+			break;
+		} else if (err < 0) {
+			pr_debug("%s: Mapping failed, will try again\n",
+				__func__);
+			break;
+		} else if (err == 0) {
+			pr_debug("%s: Mapping finished\n", __func__);
+			break;
+		}
+	}
+
+	return err;
+}
+
+/**
+ * Indicate to the ADSP that unmapping has completed, and wait for the response
+ * that its bandwidth vote has been removed.
+ *
+ * @return 0 Upon success; < 0 upon error
+ */
+static int sns_ocmem_unmap_send(void)
+{
+	int err;
+	struct sns_ocmem_hdr_s msg_hdr;
+	struct sns_ocmem_bw_vote_req_msg_v01 msg;
+	pr_debug("%s\n", __func__);
+
+	memset(&msg, 0, sizeof(struct sns_ocmem_bw_vote_req_msg_v01));
+
+	msg_hdr.msg_id = SNS_OCMEM_BW_VOTE_REQ_V01;
+	msg_hdr.msg_type = SNS_OCMEM_MSG_TYPE_REQ;
+	msg_hdr.msg_size = sizeof(struct sns_ocmem_bw_vote_req_msg_v01);
+	msg.is_map = 0;
+	msg.vectors_valid = 0;
+	msg.vectors_len = 0;
+
+	pr_debug("%s: send bw_vote OFF\n", __func__);
+	err = sns_ocmem_send_msg(&msg_hdr, &msg);
+	if (err != 0) {
+		pr_err("%s: sns_ocmem_send_msg failed %i\n",
+				__func__, err);
+	} else {
+		err = sns_ocmem_wait(DSPS_BW_VOTE_OFF, 0);
+		if (err != 0)
+			pr_err("%s: sns_ocmem_wait failed %i\n", __func__, err);
+	}
+
+	return err;
+}
+
+/**
+ * Indicate to the ADSP that mapping has completed, and wait for the response
+ * that its bandwidth vote has been made.
+ *
+ * @return 0 Upon success; < 0 upon error
+ */
+static int sns_ocmem_map_send(void)
+{
+	int err;
+	struct sns_ocmem_hdr_s msg_hdr;
+	struct sns_ocmem_bw_vote_req_msg_v01 msg;
+	struct ocmem_vectors *vectors;
+	pr_debug("%s\n", __func__);
+
+	memset(&msg, 0, sizeof(struct sns_ocmem_bw_vote_req_msg_v01));
+
+	msg_hdr.msg_id = SNS_OCMEM_BW_VOTE_REQ_V01;
+	msg_hdr.msg_type = SNS_OCMEM_MSG_TYPE_REQ;
+	msg_hdr.msg_size = sizeof(struct sns_ocmem_bw_vote_req_msg_v01);
+	msg.is_map = 1;
+
+	vectors = ocmem_get_vectors(SNS_OCMEM_CLIENT_ID, sns_ctl.buf);
+	if ((vectors != NULL)) {
+		memcpy(&msg.vectors, vectors, sizeof(vectors));
+		/* TODO: set vectors_len */
+		msg.vectors_valid = true;
+		msg.vectors_len = 0;
+	}
+
+	pr_debug("%s: send bw_vote ON\n", __func__);
+	err = sns_ocmem_send_msg(&msg_hdr, &msg);
+	if (err != 0) {
+		pr_err("%s: sns_ocmem_send_msg failed %i\n", __func__, err);
+	} else {
+		err = sns_ocmem_wait(DSPS_BW_VOTE_ON |
+					SNS_OCMEM_ALLOC_SHRINK, 0);
+		if (err != 0)
+			pr_err("%s: sns_ocmem_wait failed %i\n", __func__, err);
+	}
+
+	return err;
+}
+
+/**
+ * Perform the encessary operations to clean-up OCMEM after being notified that
+ * there is no longer a client; if sensors was evicted; or if some error
+ * has occurred.
+ *
+ * @param[i] do_free Whether the memory should be freed (true) or if shrink
+ *                   should be called instead (false).
+ */
+static void sns_ocmem_evicted(bool do_free)
+{
+	int err = 0;
+	pr_debug("%s\n", __func__);
+
+	sns_ocmem_unmap();
+	if (do_free) {
+		ocmem_free(SNS_OCMEM_CLIENT_ID, sns_ctl.buf);
+		sns_ctl.buf = NULL;
+	} else {
+		err = ocmem_shrink(SNS_OCMEM_CLIENT_ID, sns_ctl.buf, 0);
+		BUG_ON(err != 0);
+	}
+
+	err = sns_ocmem_unmap_send();
+	if (err != 0)
+		pr_err("sns_ocmem_unmap_send failed %i\n", err);
+}
+
+/**
+ * After mapping has completed and the ADSP has reacted appropriately, wait
+ * for a shrink command or word from the ADSP that it no longer has a client.
+ *
+ * @return 0 If no clients; < 0 upon error;
+ */
+static int sns_ocmem_map_done(void)
+{
+	int err = 0;
+	unsigned long flags;
+
+	pr_debug("%s\n", __func__);
+
+	err = sns_ocmem_map_send();
+	if (err != 0) {
+		pr_err("sns_ocmem_map_send failed %i\n", err);
+		sns_ocmem_evicted(true);
+	} else {
+		ocmem_set_power_state(SNS_OCMEM_CLIENT_ID,
+					sns_ctl.buf, OCMEM_OFF);
+
+		pr_debug("%s: Waiting for shrink or 'no client' updates\n",
+			__func__);
+		err = sns_ocmem_wait(DSPS_HAS_NO_CLIENT |
+					SNS_OCMEM_ALLOC_SHRINK, 0);
+		if (err == 0) {
+			if (sns_ocmem_is_status_set(DSPS_HAS_NO_CLIENT)) {
+				pr_debug("%s: No longer have a client\n",
+					__func__);
+				sns_ocmem_evicted(true);
+			} else if (sns_ocmem_is_status_set(
+						SNS_OCMEM_ALLOC_SHRINK)) {
+				pr_debug("%s: Received SHRINK\n", __func__);
+				sns_ocmem_evicted(false);
+
+				spin_lock_irqsave(&sns_ctl.sns_lock, flags);
+				sns_ctl.sns_ocmem_status &=
+						~SNS_OCMEM_ALLOC_SHRINK;
+				spin_unlock_irqrestore(&sns_ctl.sns_lock,
+							flags);
+				err = -EFAULT;
+			}
+		} else {
+			pr_err("sns_ocmem_wait failed %i\n", err);
+		}
+	}
+
+	return err;
+}
+
+/**
+ * Main function.
+ * Initializes sensors ocmem feature, and waits for an ADSP client.
+ *
+ */
+static void sns_ocmem_main(struct work_struct *work)
+{
+	int err = 0;
+	pr_debug("%s\n", __func__);
+
+	err = sns_ocmem_init();
+	if (err != 0) {
+		pr_err("%s: sns_ocmem_init failed %i\n", __func__, err);
+		return;
+	}
+
+	while (true) {
+		pr_debug("%s: Waiting for sensor client\n", __func__);
+		if (sns_ocmem_is_status_set(DSPS_HAS_CLIENT) ||
+			!sns_ocmem_wait(DSPS_HAS_CLIENT, 0)) {
+			pr_debug("%s: DSPS_HAS_CLIENT\n", __func__);
+
+			err = sns_ocmem_alloc();
+			if (err != 0) {
+				pr_err("sns_ocmem_alloc failed %i\n", err);
+				return;
+			} else {
+				err = sns_ocmem_map_done();
+				if (err != 0) {
+					pr_err("sns_ocmem_map_done failed %i",
+						err);
+					return;
+				}
+			}
+		}
+	}
+
+	ocmem_notifier_unregister(sns_ctl.ocmem_handle,
+					&sns_ctl.ocmem_nb);
+}
+
+static int sensors_adsp_open(struct inode *ip, struct file *fp)
+{
+	int ret = 0;
+	pr_debug("%s\n", __func__);
+	return ret;
+}
+
+static int sensors_adsp_release(struct inode *inode, struct file *file)
+{
+	pr_debug("%s\n", __func__);
+	return 0;
+}
+
+/**
+ * Read QTimer clock ticks and scale down to 32KHz clock as used
+ * in DSPS
+ */
+static u32 sns_read_qtimer(void)
+{
+	u64 val;
+	val = arch_counter_get_cntpct();
+	/*
+	 * To convert ticks from 19.2 Mhz clock to 32768 Hz clock:
+	 * x = (value * 32768) / 19200000
+	 * This is same as first left shift the value by 4 bits, i.e. mutiply
+	 * by 16, and then divide by 9375. The latter is preferable since
+	 * QTimer tick (value) is 56-bit, so (value * 32768) could overflow,
+	 * while (value * 16) will never do
+	 */
+	val <<= 4;
+	do_div(val, 9375);
+
+	pr_debug("%s.count=%llu\n", __func__, val);
+	return (u32)val;
+}
+
+/**
+ * IO Control - handle commands from client.
+ *
+ */
+static long sensors_adsp_ioctl(struct file *file,
+			unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+	u32 val = 0;
+	pr_debug("%s\n", __func__);
+
+	switch (cmd) {
+	case DSPS_IOCTL_READ_SLOW_TIMER:
+		val = sns_read_qtimer();
+		ret = put_user(val, (u32 __user *) arg);
+		break;
+
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * platform driver
+ *
+ */
+const struct file_operations sensors_adsp_fops = {
+	.owner = THIS_MODULE,
+	.open = sensors_adsp_open,
+	.release = sensors_adsp_release,
+	.unlocked_ioctl = sensors_adsp_ioctl,
+};
+
+static int sensors_adsp_probe(struct platform_device *pdev)
+{
+	int ret = 0;
+	pr_debug("%s.\n", __func__);
+	sns_ctl.dev_class = class_create(THIS_MODULE, DRV_NAME);
+	if (sns_ctl.dev_class == NULL) {
+		pr_err("%s: class_create fail.\n", __func__);
+		goto res_err;
+	}
+
+	ret = alloc_chrdev_region(&sns_ctl.dev_num, 0, 1, DRV_NAME);
+	if (ret) {
+		pr_err("%s: alloc_chrdev_region fail.\n", __func__);
+		goto alloc_chrdev_region_err;
+	}
+
+	sns_ctl.dev = device_create(sns_ctl.dev_class, NULL,
+				     sns_ctl.dev_num,
+				     &sns_ctl, DRV_NAME);
+	if (IS_ERR(sns_ctl.dev)) {
+		pr_err("%s: device_create fail.\n", __func__);
+		goto device_create_err;
+	}
+
+	sns_ctl.cdev = cdev_alloc();
+	if (sns_ctl.cdev == NULL) {
+		pr_err("%s: cdev_alloc fail.\n", __func__);
+		goto cdev_alloc_err;
+	}
+	cdev_init(sns_ctl.cdev, &sensors_adsp_fops);
+	sns_ctl.cdev->owner = THIS_MODULE;
+
+	ret = cdev_add(sns_ctl.cdev, sns_ctl.dev_num, 1);
+	if (ret) {
+		pr_err("%s: cdev_add fail.\n", __func__);
+		goto cdev_add_err;
+	}
+
+	sns_ctl.sns_workqueue =
+			alloc_workqueue("sns_ocmem", WQ_NON_REENTRANT, 0);
+	if (!sns_ctl.sns_workqueue) {
+		pr_err("%s: Failed to create work queue\n",
+			__func__);
+		goto cdev_add_err;
+	}
+
+	init_waitqueue_head(&sns_ctl.sns_wait);
+	spin_lock_init(&sns_ctl.sns_lock);
+
+	sns_ctl.ocmem_handle = NULL;
+	sns_ctl.buf = NULL;
+	sns_ctl.sns_ocmem_status = 0;
+	sns_ctl.ocmem_enabled = true;
+	sns_ctl.ocmem_nb.notifier_call = sns_ocmem_drv_cb;
+	sns_ctl.smd_ch = NULL;
+	sns_ctl.pdev = pdev;
+
+	INIT_WORK(&sns_ctl.sns_work, sns_ocmem_main);
+	queue_work(sns_ctl.sns_workqueue, &sns_ctl.sns_work);
+
+	return 0;
+
+cdev_add_err:
+	kfree(sns_ctl.cdev);
+cdev_alloc_err:
+	device_destroy(sns_ctl.dev_class, sns_ctl.dev_num);
+device_create_err:
+	unregister_chrdev_region(sns_ctl.dev_num, 1);
+alloc_chrdev_region_err:
+	class_destroy(sns_ctl.dev_class);
+res_err:
+	return -ENODEV;
+}
+
+static int sensors_adsp_remove(struct platform_device *pdev)
+{
+	struct msm_bus_scale_pdata *sns_ocmem_bus_scale_pdata = NULL;
+	pr_debug("%s.\n", __func__);
+
+	sns_ocmem_bus_scale_pdata = (struct msm_bus_scale_pdata *)
+					dev_get_drvdata(&pdev->dev);
+
+	kfree(sns_ocmem_bus_scale_pdata->usecase->vectors);
+	kfree(sns_ocmem_bus_scale_pdata->usecase);
+	kfree(sns_ocmem_bus_scale_pdata);
+
+	ocmem_notifier_unregister(sns_ctl.ocmem_handle,
+					&sns_ctl.ocmem_nb);
+	destroy_workqueue(sns_ctl.sns_workqueue);
+
+	cdev_del(sns_ctl.cdev);
+	kfree(sns_ctl.cdev);
+	sns_ctl.cdev = NULL;
+	device_destroy(sns_ctl.dev_class, sns_ctl.dev_num);
+	unregister_chrdev_region(sns_ctl.dev_num, 1);
+	class_destroy(sns_ctl.dev_class);
+
+	return 0;
+}
+
+static const struct of_device_id msm_adsp_sensors_dt_match[] = {
+	{.compatible = "qcom,msm-adsp-sensors"},
+	{}
+};
+MODULE_DEVICE_TABLE(of, msm_adsp_sensors_dt_match);
+
+
+static struct platform_driver sensors_adsp_driver = {
+	.driver = {
+		.name = "sensors-adsp",
+		.owner = THIS_MODULE,
+		.of_match_table = msm_adsp_sensors_dt_match,
+	},
+	.probe = sensors_adsp_probe,
+	.remove = sensors_adsp_remove,
+};
+
+/**
+ * Module Init.
+ */
+static int sensors_adsp_init(void)
+{
+	int rc;
+	pr_debug("%s.\n", __func__);
+	pr_debug("%s driver version %s.\n", DRV_NAME, DRV_VERSION);
+
+	rc = platform_driver_register(&sensors_adsp_driver);
+
+	if (rc) {
+		pr_err("%s: Failed to register sensors adsp driver\n",
+			__func__);
+		return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * Module Exit.
+ */
+static void sensors_adsp_exit(void)
+{
+	pr_debug("%s.\n", __func__);
+	platform_driver_unregister(&sensors_adsp_driver);
+}
+
+module_init(sensors_adsp_init);
+module_exit(sensors_adsp_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Sensors ADSP driver");
diff --git a/arch/arm/mach-msm/spm.h b/arch/arm/mach-msm/spm.h
index a353ce0..01f6787 100644
--- a/arch/arm/mach-msm/spm.h
+++ b/arch/arm/mach-msm/spm.h
@@ -130,7 +130,14 @@
 int msm_spm_set_low_power_mode(unsigned int mode, bool notify_rpm);
 int msm_spm_set_vdd(unsigned int cpu, unsigned int vlevel);
 unsigned int msm_spm_get_vdd(unsigned int cpu);
+#if defined(CONFIG_MSM_SPM_V2)
 int msm_spm_turn_on_cpu_rail(unsigned int cpu);
+#else
+static inline int msm_spm_turn_on_cpu_rail(unsigned int cpu)
+{
+	return -ENOSYS;
+}
+#endif
 
 /* Internal low power management specific functions */
 
diff --git a/arch/arm/mach-msm/spm_devices.c b/arch/arm/mach-msm/spm_devices.c
index b378d3b..e77a7ac 100644
--- a/arch/arm/mach-msm/spm_devices.c
+++ b/arch/arm/mach-msm/spm_devices.c
@@ -374,7 +374,7 @@
 	};
 
 	struct mode_of of_l2_modes[] = {
-		{"qcom,saw2-spm-cmd-ret", MSM_SPM_L2_MODE_RETENTION, 1},
+		{"qcom,saw2-spm-cmd-ret", MSM_SPM_L2_MODE_RETENTION, 0},
 		{"qcom,saw2-spm-cmd-gdhs", MSM_SPM_L2_MODE_GDHS, 1},
 		{"qcom,saw2-spm-cmd-pc", MSM_SPM_L2_MODE_POWER_COLLAPSE, 1},
 	};
diff --git a/arch/arm/mach-msm/subsystem_restart.c b/arch/arm/mach-msm/subsystem_restart.c
index ea9337d..d862e0f 100644
--- a/arch/arm/mach-msm/subsystem_restart.c
+++ b/arch/arm/mach-msm/subsystem_restart.c
@@ -124,6 +124,7 @@
  * @id: ida
  * @restart_order: order of other devices this devices restarts with
  * @dentry: debugfs directory for this device
+ * @do_ramdump_on_put: ramdump on subsystem_put() if true
  */
 struct subsys_device {
 	struct subsys_desc *desc;
@@ -142,6 +143,7 @@
 #ifdef CONFIG_DEBUG_FS
 	struct dentry *dentry;
 #endif
+	bool do_ramdump_on_put;
 };
 
 static struct subsys_device *to_subsys(struct device *d)
@@ -427,6 +429,7 @@
 	if (dev->desc->ramdump)
 		if (dev->desc->ramdump(enable_ramdumps, dev->desc) < 0)
 			pr_warn("%s[%p]: Ramdump failed.\n", name, current);
+	dev->do_ramdump_on_put = false;
 }
 
 static void subsystem_powerup(struct subsys_device *dev, void *data)
@@ -561,8 +564,11 @@
 	if (WARN(!subsys->count, "%s: %s: Reference count mismatch\n",
 			subsys->desc->name, __func__))
 		goto err_out;
-	if (!--subsys->count)
+	if (!--subsys->count) {
 		subsys_stop(subsys);
+		if (subsys->do_ramdump_on_put)
+			subsystem_ramdump(subsys, NULL);
+	}
 	mutex_unlock(&track->lock);
 
 	subsys_d = find_subsys(subsys->desc->depends_on);
@@ -733,6 +739,33 @@
 }
 EXPORT_SYMBOL(subsystem_restart);
 
+int subsystem_crashed(const char *name)
+{
+	struct subsys_device *dev = find_subsys(name);
+	struct subsys_tracking *track;
+
+	if (!dev)
+		return -ENODEV;
+
+	if (!get_device(&dev->dev))
+		return -ENODEV;
+
+	track = subsys_get_track(dev);
+
+	mutex_lock(&track->lock);
+	dev->do_ramdump_on_put = true;
+	/*
+	 * TODO: Make this work with multiple consumers where one is calling
+	 * subsystem_restart() and another is calling this function. To do
+	 * so would require updating private state, etc.
+	 */
+	mutex_unlock(&track->lock);
+
+	put_device(&dev->dev);
+	return 0;
+}
+EXPORT_SYMBOL(subsystem_crashed);
+
 #ifdef CONFIG_DEBUG_FS
 static ssize_t subsys_debugfs_read(struct file *filp, char __user *ubuf,
 		size_t cnt, loff_t *ppos)
diff --git a/arch/arm/mach-msm/timer.c b/arch/arm/mach-msm/timer.c
index 212ad77..e360906 100644
--- a/arch/arm/mach-msm/timer.c
+++ b/arch/arm/mach-msm/timer.c
@@ -1035,7 +1035,8 @@
 
 	if (cpu_is_msm7x01() || cpu_is_msm7x25() || cpu_is_msm7x27() ||
 	    cpu_is_msm7x25a() || cpu_is_msm7x27a() || cpu_is_msm7x25aa() ||
-	    cpu_is_msm7x27aa() || cpu_is_msm8625() || cpu_is_msm7x25ab()) {
+	    cpu_is_msm7x27aa() || cpu_is_msm8625() || cpu_is_msm7x25ab() ||
+	    cpu_is_msm8625q()) {
 		dgt->shift = MSM_DGT_SHIFT;
 		dgt->freq = 19200000 >> MSM_DGT_SHIFT;
 		dgt->clockevent.shift = 32 + MSM_DGT_SHIFT;
@@ -1045,7 +1046,7 @@
 		gpt->flags |= MSM_CLOCK_FLAGS_UNSTABLE_COUNT
 			   |  MSM_CLOCK_FLAGS_ODD_MATCH_WRITE
 			   |  MSM_CLOCK_FLAGS_DELAYED_WRITE_POST;
-		if (cpu_is_msm8625())
+		if (cpu_is_msm8625() || cpu_is_msm8625q())
 			fixup_msm8625_timer();
 	} else if (cpu_is_qsd8x50()) {
 		dgt->freq = 4800000;
@@ -1134,8 +1135,8 @@
 
 		ce->irq = clock->irq;
 		if (cpu_is_msm8x60() || cpu_is_msm9615() || cpu_is_msm8625() ||
-		    soc_class_is_msm8960() || soc_class_is_apq8064() ||
-		    soc_class_is_msm8930()) {
+		    cpu_is_msm8625q() || soc_class_is_msm8960() ||
+		    soc_class_is_apq8064() || soc_class_is_msm8930()) {
 			clock->percpu_evt = alloc_percpu(struct clock_event_device *);
 			if (!clock->percpu_evt) {
 				pr_err("msm_timer_init: memory allocation "
diff --git a/block/blk-core.c b/block/blk-core.c
index 68d7158..fe1c7e0 100644
--- a/block/blk-core.c
+++ b/block/blk-core.c
@@ -297,13 +297,26 @@
  * Description:
  *    See @blk_run_queue. This variant must be called with the queue lock
  *    held and interrupts disabled.
+ *    Device driver will be notified of an urgent request
+ *    pending under the following conditions:
+ *    1. The driver and the current scheduler support urgent reques handling
+ *    2. There is an urgent request pending in the scheduler
+ *    3. There isn't already an urgent request in flight, meaning previously
+ *       notified urgent request completed (!q->notified_urgent)
  */
 void __blk_run_queue(struct request_queue *q)
 {
 	if (unlikely(blk_queue_stopped(q)))
 		return;
 
-	q->request_fn(q);
+	if (!q->notified_urgent &&
+		q->elevator->type->ops.elevator_is_urgent_fn &&
+		q->urgent_request_fn &&
+		q->elevator->type->ops.elevator_is_urgent_fn(q)) {
+		q->notified_urgent = true;
+		q->urgent_request_fn(q);
+	} else
+		q->request_fn(q);
 }
 EXPORT_SYMBOL(__blk_run_queue);
 
@@ -1070,6 +1083,50 @@
 }
 EXPORT_SYMBOL(blk_requeue_request);
 
+/**
+ * blk_reinsert_request() - Insert a request back to the scheduler
+ * @q:		request queue
+ * @rq:		request to be inserted
+ *
+ * This function inserts the request back to the scheduler as if
+ * it was never dispatched.
+ *
+ * Return: 0 on success, error code on fail
+ */
+int blk_reinsert_request(struct request_queue *q, struct request *rq)
+{
+	if (unlikely(!rq) || unlikely(!q))
+		return -EIO;
+
+	blk_delete_timer(rq);
+	blk_clear_rq_complete(rq);
+	trace_block_rq_requeue(q, rq);
+
+	if (blk_rq_tagged(rq))
+		blk_queue_end_tag(q, rq);
+
+	BUG_ON(blk_queued_rq(rq));
+
+	return elv_reinsert_request(q, rq);
+}
+EXPORT_SYMBOL(blk_reinsert_request);
+
+/**
+ * blk_reinsert_req_sup() - check whether the scheduler supports
+ *          reinsertion of requests
+ * @q:		request queue
+ *
+ * Returns true if the current scheduler supports reinserting
+ * request. False otherwise
+ */
+bool blk_reinsert_req_sup(struct request_queue *q)
+{
+	if (unlikely(!q))
+		return false;
+	return q->elevator->type->ops.elevator_reinsert_req_fn ? true : false;
+}
+EXPORT_SYMBOL(blk_reinsert_req_sup);
+
 static void add_acct_request(struct request_queue *q, struct request *rq,
 			     int where)
 {
@@ -2073,8 +2130,17 @@
 	struct request *rq;
 
 	rq = blk_peek_request(q);
-	if (rq)
+	if (rq) {
+		/*
+		 * Assumption: the next request fetched from scheduler after we
+		 * notified "urgent request pending" - will be the urgent one
+		 */
+		if (q->notified_urgent && !q->dispatched_urgent) {
+			q->dispatched_urgent = true;
+			(void)blk_mark_rq_urgent(rq);
+		}
 		blk_start_request(rq);
+	}
 	return rq;
 }
 EXPORT_SYMBOL(blk_fetch_request);
diff --git a/block/blk-settings.c b/block/blk-settings.c
index d3234fc..579328c 100644
--- a/block/blk-settings.c
+++ b/block/blk-settings.c
@@ -100,6 +100,18 @@
 EXPORT_SYMBOL_GPL(blk_queue_lld_busy);
 
 /**
+ * blk_urgent_request() - Set an urgent_request handler function for queue
+ * @q:		queue
+ * @fn:		handler for urgent requests
+ *
+ */
+void blk_urgent_request(struct request_queue *q, request_fn_proc *fn)
+{
+	q->urgent_request_fn = fn;
+}
+EXPORT_SYMBOL(blk_urgent_request);
+
+/**
  * blk_set_default_limits - reset limits to default values
  * @lim:  the queue_limits structure to reset
  *
diff --git a/block/blk.h b/block/blk.h
index d45be87..a52209f 100644
--- a/block/blk.h
+++ b/block/blk.h
@@ -39,6 +39,7 @@
  */
 enum rq_atomic_flags {
 	REQ_ATOM_COMPLETE = 0,
+	REQ_ATOM_URGENT = 1,
 };
 
 /*
@@ -55,6 +56,16 @@
 	clear_bit(REQ_ATOM_COMPLETE, &rq->atomic_flags);
 }
 
+static inline int blk_mark_rq_urgent(struct request *rq)
+{
+	return test_and_set_bit(REQ_ATOM_URGENT, &rq->atomic_flags);
+}
+
+static inline void blk_clear_rq_urgent(struct request *rq)
+{
+	clear_bit(REQ_ATOM_URGENT, &rq->atomic_flags);
+}
+
 /*
  * Internal elevator interface
  */
diff --git a/block/cfq-iosched.c b/block/cfq-iosched.c
index 3c38536..32629e2 100644
--- a/block/cfq-iosched.c
+++ b/block/cfq-iosched.c
@@ -2305,6 +2305,9 @@
 {
 	struct cfq_group *cfqg = cfq_get_next_cfqg(cfqd);
 
+	if (!cfqg)
+		return;
+
 	cfqd->serving_group = cfqg;
 
 	/* Restore the workload type data */
diff --git a/block/elevator.c b/block/elevator.c
index 74fd51b..efec457 100644
--- a/block/elevator.c
+++ b/block/elevator.c
@@ -585,6 +585,41 @@
 	__elv_add_request(q, rq, ELEVATOR_INSERT_REQUEUE);
 }
 
+/**
+ * elv_reinsert_request() - Insert a request back to the scheduler
+ * @q:		request queue where request should be inserted
+ * @rq:		request to be inserted
+ *
+ * This function returns the request back to the scheduler to be
+ * inserted as if it was never dispatched
+ *
+ * Return: 0 on success, error code on failure
+ */
+int elv_reinsert_request(struct request_queue *q, struct request *rq)
+{
+	int res;
+
+	if (!q->elevator->type->ops.elevator_reinsert_req_fn)
+		return -EPERM;
+
+	res = q->elevator->type->ops.elevator_reinsert_req_fn(q, rq);
+	if (!res) {
+		/*
+		 * it already went through dequeue, we need to decrement the
+		 * in_flight count again
+		 */
+		if (blk_account_rq(rq)) {
+			q->in_flight[rq_is_sync(rq)]--;
+			if (rq->cmd_flags & REQ_SORTED)
+				elv_deactivate_rq(q, rq);
+		}
+		rq->cmd_flags &= ~REQ_STARTED;
+		q->nr_sorted++;
+	}
+
+	return res;
+}
+
 void elv_drain_elevator(struct request_queue *q)
 {
 	static int printed;
@@ -779,6 +814,11 @@
 {
 	struct elevator_queue *e = q->elevator;
 
+	if (test_bit(REQ_ATOM_URGENT, &rq->atomic_flags)) {
+		q->notified_urgent = false;
+		q->dispatched_urgent = false;
+		blk_clear_rq_urgent(rq);
+	}
 	/*
 	 * request is released from the driver, io must be done
 	 */
diff --git a/block/row-iosched.c b/block/row-iosched.c
index b7965c6..483d97f 100644
--- a/block/row-iosched.c
+++ b/block/row-iosched.c
@@ -69,20 +69,19 @@
 	1	/* ROWQ_PRIO_LOW_SWRITE */
 };
 
-/* Default values for idling on read queues */
-#define ROW_IDLE_TIME 50	/* 5 msec */
-#define ROW_READ_FREQ 70	/* 7 msec */
+/* Default values for idling on read queues (in msec) */
+#define ROW_IDLE_TIME_MSEC 5
+#define ROW_READ_FREQ_MSEC 20
 
 /**
  * struct rowq_idling_data -  parameters for idling on the queue
- * @idle_trigger_time:	time (in jiffies). If a new request was
- *			inserted before this time value, idling
- *			will be enabled.
+ * @last_insert_time:	time the last request was inserted
+ *			to the queue
  * @begin_idling:	flag indicating wether we should idle
  *
  */
 struct rowq_idling_data {
-	unsigned long		idle_trigger_time;
+	ktime_t			last_insert_time;
 	bool			begin_idling;
 };
 
@@ -111,7 +110,7 @@
 
 /**
  * struct idling_data - data for idling on empty rqueue
- * @idle_time:		idling duration (msec)
+ * @idle_time:		idling duration (jiffies)
  * @freq:		min time between two requests that
  *			triger idling (msec)
  * @idle_work:		pointer to struct delayed_work
@@ -119,7 +118,7 @@
  */
 struct idling_data {
 	unsigned long			idle_time;
-	unsigned long			freq;
+	u32				freq;
 
 	struct workqueue_struct	*idle_workqueue;
 	struct delayed_work		idle_work;
@@ -260,14 +259,17 @@
 		if (delayed_work_pending(&rd->read_idle.idle_work))
 			(void)cancel_delayed_work(
 				&rd->read_idle.idle_work);
-		if (time_before(jiffies, rqueue->idle_data.idle_trigger_time)) {
+		if (ktime_to_ms(ktime_sub(ktime_get(),
+				rqueue->idle_data.last_insert_time)) <
+				rd->read_idle.freq) {
 			rqueue->idle_data.begin_idling = true;
 			row_log_rowq(rd, rqueue->prio, "Enable idling");
-		} else
+		} else {
 			rqueue->idle_data.begin_idling = false;
+			row_log_rowq(rd, rqueue->prio, "Disable idling");
+		}
 
-		rqueue->idle_data.idle_trigger_time =
-			jiffies + msecs_to_jiffies(rd->read_idle.freq);
+		rqueue->idle_data.last_insert_time = ktime_get();
 	}
 	row_log_rowq(rd, rqueue->prio, "added request");
 }
@@ -402,9 +404,8 @@
 		if (!force && queue_idling_enabled[currq] &&
 		    rd->row_queues[currq].rqueue.idle_data.begin_idling) {
 			if (!queue_delayed_work(rd->read_idle.idle_workqueue,
-			    &rd->read_idle.idle_work,
-			    jiffies +
-			    msecs_to_jiffies(rd->read_idle.idle_time))) {
+						&rd->read_idle.idle_work,
+						rd->read_idle.idle_time)) {
 				row_log_rowq(rd, currq,
 					     "Work already on queue!");
 				pr_err("ROW_BUG: Work already on queue!");
@@ -453,6 +454,8 @@
 		rdata->row_queues[i].rqueue.rdata = rdata;
 		rdata->row_queues[i].rqueue.prio = i;
 		rdata->row_queues[i].rqueue.idle_data.begin_idling = false;
+		rdata->row_queues[i].rqueue.idle_data.last_insert_time =
+			ktime_set(0, 0);
 	}
 
 	/*
@@ -460,8 +463,11 @@
 	 * enable it for write queues also, note that idling frequency will
 	 * be the same in both cases
 	 */
-	rdata->read_idle.idle_time = ROW_IDLE_TIME;
-	rdata->read_idle.freq = ROW_READ_FREQ;
+	rdata->read_idle.idle_time = msecs_to_jiffies(ROW_IDLE_TIME_MSEC);
+	/* Maybe 0 on some platforms */
+	if (!rdata->read_idle.idle_time)
+		rdata->read_idle.idle_time = 1;
+	rdata->read_idle.freq = ROW_READ_FREQ_MSEC;
 	rdata->read_idle.idle_workqueue = alloc_workqueue("row_idle_work",
 					    WQ_MEM_RECLAIM | WQ_HIGHPRI, 0);
 	if (!rdata->read_idle.idle_workqueue)
@@ -489,6 +495,8 @@
 	for (i = 0; i < ROWQ_MAX_PRIO; i++)
 		BUG_ON(!list_empty(&rd->row_queues[i].rqueue.fifo));
 	(void)cancel_delayed_work_sync(&rd->read_idle.idle_work);
+	BUG_ON(delayed_work_pending(&rd->read_idle.idle_work));
+	destroy_workqueue(rd->read_idle.idle_workqueue);
 	kfree(rd);
 }
 
@@ -590,7 +598,7 @@
 SHOW_FUNCTION(row_lp_swrite_quantum_show,
 	rowd->row_queues[ROWQ_PRIO_LOW_SWRITE].disp_quantum, 0);
 SHOW_FUNCTION(row_read_idle_show, rowd->read_idle.idle_time, 1);
-SHOW_FUNCTION(row_read_idle_freq_show, rowd->read_idle.freq, 1);
+SHOW_FUNCTION(row_read_idle_freq_show, rowd->read_idle.freq, 0);
 #undef SHOW_FUNCTION
 
 #define STORE_FUNCTION(__FUNC, __PTR, MIN, MAX, __CONV)			\
@@ -630,7 +638,7 @@
 			&rowd->row_queues[ROWQ_PRIO_LOW_SWRITE].disp_quantum,
 			1, INT_MAX, 1);
 STORE_FUNCTION(row_read_idle_store, &rowd->read_idle.idle_time, 1, INT_MAX, 1);
-STORE_FUNCTION(row_read_idle_freq_store, &rowd->read_idle.freq, 1, INT_MAX, 1);
+STORE_FUNCTION(row_read_idle_freq_store, &rowd->read_idle.freq, 1, INT_MAX, 0);
 
 #undef STORE_FUNCTION
 
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index 9e419e1..82d19e0 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -281,6 +281,7 @@
 static const struct of_device_id ahci_of_match[] = {
 	{ .compatible = "calxeda,hb-ahci", },
 	{ .compatible = "snps,spear-ahci", },
+	{ .compatible = "qcom,msm-ahci", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ahci_of_match);
diff --git a/drivers/char/diag/diag_dci.c b/drivers/char/diag/diag_dci.c
index 24fc99a..d4b1856 100644
--- a/drivers/char/diag/diag_dci.c
+++ b/drivers/char/diag/diag_dci.c
@@ -38,73 +38,48 @@
 struct mutex dci_log_mask_mutex;
 struct mutex dci_event_mask_mutex;
 
-smd_channel_t *ch_dci_temp;
-
 #define DCI_CHK_CAPACITY(entry, new_data_len)				\
 ((entry->data_len + new_data_len > entry->total_capacity) ? 1 : 0)	\
 
-static void diag_smd_dci_send_req(int proc_num)
+/* Process the data read from the smd dci channel */
+int diag_process_smd_dci_read_data(struct diag_smd_info *smd_info, void *buf,
+								int recd_bytes)
 {
-	void *buf = NULL;
-	smd_channel_t *smd_ch = NULL;
-	int recd_bytes, read_bytes, dci_pkt_len, i;
+	int read_bytes, dci_pkt_len, i;
 	uint8_t recv_pkt_cmd_code;
 
-	if (driver->in_busy_dci)
-		return;
-
-	if (proc_num == MODEM_PROC) {
-		buf = driver->buf_in_dci;
-		smd_ch = driver->ch_dci;
+	/* Each SMD read can have multiple DCI packets */
+	read_bytes = 0;
+	while (read_bytes < recd_bytes) {
+		/* read actual length of dci pkt */
+		dci_pkt_len = *(uint16_t *)(buf+2);
+		/* process one dci packet */
+		pr_debug("diag: bytes read = %d, single dci pkt len = %d\n",
+			read_bytes, dci_pkt_len);
+		/* print_hex_dump(KERN_DEBUG, "Single DCI packet :",
+		 DUMP_PREFIX_ADDRESS, 16, 1, buf, 5 + dci_pkt_len, 1);*/
+		recv_pkt_cmd_code = *(uint8_t *)(buf+4);
+		if (recv_pkt_cmd_code == LOG_CMD_CODE)
+			extract_dci_log(buf+4);
+		else if (recv_pkt_cmd_code == EVENT_CMD_CODE)
+			extract_dci_events(buf+4);
+		else
+			extract_dci_pkt_rsp(buf); /* pkt response */
+		read_bytes += 5 + dci_pkt_len;
+		buf += 5 + dci_pkt_len; /* advance to next DCI pkt */
 	}
-
-	if (!smd_ch || !buf)
-		return;
-
-	recd_bytes = smd_read_avail(smd_ch);
-	if (recd_bytes > IN_BUF_SIZE) {
-		if (recd_bytes < MAX_IN_BUF_SIZE) {
-			pr_err("diag: SMD DCI sending pkt upto %d bytes",
-				recd_bytes);
-			buf = krealloc(buf, recd_bytes, GFP_KERNEL);
-		} else {
-			pr_err("diag: DCI pkt > %d bytes", MAX_IN_BUF_SIZE);
-			return;
+	/* wake up all sleeping DCI clients which have some data */
+	for (i = 0; i < MAX_DCI_CLIENTS; i++) {
+		if (driver->dci_client_tbl[i].client &&
+			driver->dci_client_tbl[i].data_len) {
+			smd_info->in_busy_1 = 1;
+			diag_update_sleeping_process(
+				driver->dci_client_tbl[i].client->tgid,
+					 DCI_DATA_TYPE);
 		}
 	}
-	if (buf && recd_bytes > 0) {
-		smd_read(smd_ch, buf, recd_bytes);
-		pr_debug("diag: data received %d bytes\n", recd_bytes);
-		/* Each SMD read can have multiple DCI packets */
-		read_bytes = 0;
-		while (read_bytes < recd_bytes) {
-			/* read actual length of dci pkt */
-			dci_pkt_len = *(uint16_t *)(buf+2);
-			/* process one dci packet */
-			pr_debug("diag: bytes read = %d, single dci pkt len = %d\n",
-				read_bytes, dci_pkt_len);
-			/* print_hex_dump(KERN_DEBUG, "Single DCI packet :",
-			 DUMP_PREFIX_ADDRESS, 16, 1, buf, 5 + dci_pkt_len, 1);*/
-			recv_pkt_cmd_code = *(uint8_t *)(buf+4);
-			if (recv_pkt_cmd_code == LOG_CMD_CODE)
-				extract_dci_log(buf+4);
-			else if (recv_pkt_cmd_code == EVENT_CMD_CODE)
-				extract_dci_events(buf+4);
-			else
-				extract_dci_pkt_rsp(buf); /* pkt response */
-			read_bytes += 5 + dci_pkt_len;
-			buf += 5 + dci_pkt_len; /* advance to next DCI pkt */
-		}
-		/* wake up all sleeping DCI clients which have some data */
-		for (i = 0; i < MAX_DCI_CLIENTS; i++)
-			if (driver->dci_client_tbl[i].client &&
-				driver->dci_client_tbl[i].data_len) {
-				driver->in_busy_dci = 1;
-				diag_update_sleeping_process(
-					driver->dci_client_tbl[i].client->tgid,
-						 DCI_DATA_TYPE);
-			}
-	}
+
+	return 0;
 }
 
 void extract_dci_pkt_rsp(unsigned char *buf)
@@ -163,7 +138,8 @@
 					buf+4+cmd_code_len, write_len);
 				entry->data_len += write_len;
 				/* delete immediate response entry */
-				if (driver->buf_in_dci[8+cmd_code_len] != 0x80)
+				if (driver->smd_dci[MODEM_DATA].
+					buf_in_1[8+cmd_code_len] != 0x80)
 					driver->req_tracking_tbl[index].pid = 0;
 				break;
 			}
@@ -303,23 +279,22 @@
 	}
 }
 
-void diag_read_smd_dci_work_fn(struct work_struct *work)
-{
-	diag_smd_dci_send_req(MODEM_PROC);
-}
-
 void diag_update_smd_dci_work_fn(struct work_struct *work)
 {
+	struct diag_smd_info *smd_info = container_of(work,
+						struct diag_smd_info,
+						diag_notify_update_smd_work);
 	int i, j;
 	char dirty_bits[16];
 	uint8_t *client_log_mask_ptr;
 	uint8_t *log_mask_ptr;
 	int ret;
+	int index = smd_info->peripheral;
 
 	/* Update the peripheral(s) with the dci log and event masks */
 
 	/* If the cntl channel is not up, we can't update logs and events */
-	if (!driver->ch_cntl)
+	if (!driver->smd_cntl[index].ch)
 		return;
 
 	memset(dirty_bits, 0, 16 * sizeof(uint8_t));
@@ -352,9 +327,11 @@
 	}
 	mutex_unlock(&dci_log_mask_mutex);
 
-	ret = diag_send_dci_log_mask(driver->ch_cntl);
+	ret = diag_send_dci_log_mask(driver->smd_cntl[index].ch);
 
-	ret = diag_send_dci_event_mask(driver->ch_cntl);
+	ret = diag_send_dci_event_mask(driver->smd_cntl[index].ch);
+
+	smd_info->notify_context = 0;
 }
 
 void diag_dci_notify_client(int peripheral_mask, int data)
@@ -380,41 +357,23 @@
 	} /* end of loop for all DCI clients */
 }
 
-static void diag_smd_dci_notify(void *ctxt, unsigned event)
-{
-	if (event == SMD_EVENT_CLOSE) {
-		driver->ch_dci = 0;
-		/* Notify the clients of the close */
-		diag_dci_notify_client(DIAG_CON_MPSS, DIAG_STATUS_CLOSED);
-		return;
-	} else if (event == SMD_EVENT_OPEN) {
-
-		if (ch_dci_temp)
-			driver->ch_dci = ch_dci_temp;
-
-		queue_work(driver->diag_dci_wq,
-			&(driver->diag_update_smd_dci_work));
-
-		/* Notify the clients of the open */
-		diag_dci_notify_client(DIAG_CON_MPSS, DIAG_STATUS_OPEN);
-	}
-
-	queue_work(driver->diag_dci_wq, &(driver->diag_read_smd_dci_work));
-}
-
 static int diag_dci_probe(struct platform_device *pdev)
 {
 	int err = 0;
+	int index;
 
 	if (pdev->id == SMD_APPS_MODEM) {
-		err = smd_open("DIAG_2", &driver->ch_dci, driver,
-						diag_smd_dci_notify);
+		index = MODEM_DATA;
+		err = smd_open("DIAG_2", &driver->smd_dci[index].ch,
+					&driver->smd_dci[index],
+					diag_smd_notify);
+		driver->smd_dci[index].ch_save =
+					driver->smd_dci[index].ch;
 		if (err)
-			pr_err("diag: cannot open DCI port, Id = %d, err ="
-				" %d\n", pdev->id, err);
-		else
-			ch_dci_temp = driver->ch_dci;
+			pr_err("diag: In %s, cannot open DCI port, Id = %d, err: %d\n",
+				__func__, pdev->id, err);
 	}
+
 	return err;
 }
 
@@ -422,6 +381,7 @@
 					 int len, int index)
 {
 	int i;
+	int status = 0;
 
 	/* remove UID from user space pkt before sending to peripheral */
 	buf = buf + 4;
@@ -439,15 +399,23 @@
 
 	driver->apps_dci_buf[9+len] = CONTROL_CHAR; /* end */
 
-	if (entry.client_id == MODEM_PROC && driver->ch_dci) {
-		smd_write(driver->ch_dci, driver->apps_dci_buf, len + 10);
-		i = DIAG_DCI_NO_ERROR;
-	} else {
+	for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
+		if (entry.client_id == driver->smd_dci[i].peripheral) {
+			if (driver->smd_dci[i].ch) {
+				smd_write(driver->smd_dci[i].ch,
+					driver->apps_dci_buf, len + 10);
+				status = DIAG_DCI_NO_ERROR;
+			}
+			break;
+		}
+	}
+
+	if (status != DIAG_DCI_NO_ERROR) {
 		pr_alert("diag: check DCI channel\n");
-		i = DIAG_DCI_SEND_DATA_FAIL;
+		status = DIAG_DCI_SEND_DATA_FAIL;
 	}
 	mutex_unlock(&driver->dci_mutex);
-	return i;
+	return status;
 }
 
 int diag_register_dci_transaction(int uid)
@@ -487,8 +455,9 @@
 	uint8_t *event_mask_ptr;
 	int offset = 0;
 
-	if (!driver->ch_dci) {
-		pr_err("diag: ch_dci not valid for dci updates\n");
+	if (!driver->smd_dci[MODEM_DATA].ch) {
+		pr_err("diag: DCI smd channel for peripheral %d not valid for dci updates\n",
+			driver->smd_dci[MODEM_DATA].peripheral);
 		return DIAG_DCI_SEND_DATA_FAIL;
 	}
 
@@ -613,7 +582,7 @@
 			ret = DIAG_DCI_NO_ERROR;
 		}
 		/* send updated mask to peripherals */
-		ret = diag_send_dci_log_mask(driver->ch_cntl);
+		ret = diag_send_dci_log_mask(driver->smd_cntl[MODEM_DATA].ch);
 	} else if (*(int *)temp == DCI_EVENT_TYPE) {
 		/* find client id and table */
 		for (i = 0; i < MAX_DCI_CLIENTS; i++) {
@@ -659,7 +628,7 @@
 			ret = DIAG_DCI_NO_ERROR;
 		}
 		/* send updated mask to peripherals */
-		ret = diag_send_dci_event_mask(driver->ch_cntl);
+		ret = diag_send_dci_event_mask(driver->smd_cntl[MODEM_DATA].ch);
 	} else {
 		pr_alert("diag: Incorrect DCI transaction\n");
 	}
@@ -872,27 +841,19 @@
 int diag_dci_init(void)
 {
 	int success = 0;
-
-	ch_dci_temp = NULL;
+	int i;
 
 	driver->dci_tag = 0;
 	driver->dci_client_id = 0;
 	driver->num_dci_client = 0;
-	driver->in_busy_dci = 0;
 	mutex_init(&driver->dci_mutex);
 	mutex_init(&dci_log_mask_mutex);
 	mutex_init(&dci_event_mask_mutex);
-	if (driver->buf_in_dci == NULL) {
-		driver->buf_in_dci = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_dci == NULL)
-			goto err;
-	}
-	if (driver->write_ptr_dci == NULL) {
-		driver->write_ptr_dci = kzalloc(
-			sizeof(struct diag_write_device), GFP_KERNEL);
-		if (driver->write_ptr_dci == NULL)
-			goto err;
-	}
+	success = diag_smd_constructor(&driver->smd_dci[MODEM_DATA],
+					MODEM_DATA, SMD_DCI_TYPE);
+	if (!success)
+		goto err;
+
 	if (driver->req_tracking_tbl == NULL) {
 		driver->req_tracking_tbl = kzalloc(dci_max_reg *
 			sizeof(struct dci_pkt_req_tracking_tbl), GFP_KERNEL);
@@ -922,8 +883,8 @@
 	kfree(driver->req_tracking_tbl);
 	kfree(driver->dci_client_tbl);
 	kfree(driver->apps_dci_buf);
-	kfree(driver->buf_in_dci);
-	kfree(driver->write_ptr_dci);
+	for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_dci[i]);
 	if (driver->diag_dci_wq)
 		destroy_workqueue(driver->diag_dci_wq);
 	return DIAG_DCI_NO_REG;
@@ -931,13 +892,14 @@
 
 void diag_dci_exit(void)
 {
-	smd_close(driver->ch_dci);
-	driver->ch_dci = 0;
+	int i;
+
+	for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_dci[i]);
+
 	platform_driver_unregister(&msm_diag_dci_driver);
 	kfree(driver->req_tracking_tbl);
 	kfree(driver->dci_client_tbl);
 	kfree(driver->apps_dci_buf);
-	kfree(driver->buf_in_dci);
-	kfree(driver->write_ptr_dci);
 	destroy_workqueue(driver->diag_dci_wq);
 }
diff --git a/drivers/char/diag/diag_dci.h b/drivers/char/diag/diag_dci.h
index 3f62e5e..af89ac8 100644
--- a/drivers/char/diag/diag_dci.h
+++ b/drivers/char/diag/diag_dci.h
@@ -78,8 +78,10 @@
 
 int diag_dci_init(void);
 void diag_dci_exit(void);
-void diag_read_smd_dci_work_fn(struct work_struct *);
 void diag_update_smd_dci_work_fn(struct work_struct *);
+void diag_dci_notify_client(int peripheral_mask, int data);
+int diag_process_smd_dci_read_data(struct diag_smd_info *smd_info, void *buf,
+								int recd_bytes);
 int diag_process_dci_transaction(unsigned char *buf, int len);
 int diag_send_dci_pkt(struct diag_master_table entry, unsigned char *buf,
 							 int len, int index);
diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c
index c404229..d852d75 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -48,34 +48,34 @@
 		"Check Polling Response: %d\n"
 		"polling_reg_flag: %d\n"
 		"uses device tree: %d\n"
-		"in_busy_1: %d\n"
-		"in_busy_2: %d\n"
-		"in_busy_lpass_1: %d\n"
-		"in_busy_lpass_2: %d\n"
-		"in_busy_wcnss_1: %d\n"
-		"in_busy_wcnss_2: %d\n"
-		"in_busy_dci: %d\n"
+		"Modem in_busy_1: %d\n"
+		"Modem in_busy_2: %d\n"
+		"LPASS in_busy_1: %d\n"
+		"LPASS in_busy_2: %d\n"
+		"RIVA in_busy_1: %d\n"
+		"RIVA in_busy_2: %d\n"
+		"DCI Modem in_busy_1: %d\n"
 		"logging_mode: %d\n",
-		(unsigned int)driver->ch,
-		(unsigned int)driver->chlpass,
-		(unsigned int)driver->ch_wcnss,
-		(unsigned int)driver->ch_dci,
-		(unsigned int)driver->ch_cntl,
-		(unsigned int)driver->chlpass_cntl,
-		(unsigned int)driver->ch_wcnss_cntl,
+		(unsigned int)driver->smd_data[MODEM_DATA].ch,
+		(unsigned int)driver->smd_data[LPASS_DATA].ch,
+		(unsigned int)driver->smd_data[WCNSS_DATA].ch,
+		(unsigned int)driver->smd_dci[MODEM_DATA].ch,
+		(unsigned int)driver->smd_cntl[MODEM_DATA].ch,
+		(unsigned int)driver->smd_cntl[LPASS_DATA].ch,
+		(unsigned int)driver->smd_cntl[WCNSS_DATA].ch,
 		chk_config_get_id(),
 		chk_apps_only(),
 		chk_apps_master(),
 		chk_polling_response(),
 		driver->polling_reg_flag,
 		driver->use_device_tree,
-		driver->in_busy_1,
-		driver->in_busy_2,
-		driver->in_busy_lpass_1,
-		driver->in_busy_lpass_2,
-		driver->in_busy_wcnss_1,
-		driver->in_busy_wcnss_2,
-		driver->in_busy_dci,
+		driver->smd_data[MODEM_DATA].in_busy_1,
+		driver->smd_data[MODEM_DATA].in_busy_2,
+		driver->smd_data[LPASS_DATA].in_busy_1,
+		driver->smd_data[LPASS_DATA].in_busy_2,
+		driver->smd_data[WCNSS_DATA].in_busy_1,
+		driver->smd_data[WCNSS_DATA].in_busy_2,
+		driver->smd_dci[MODEM_DATA].in_busy_1,
 		driver->logging_mode);
 
 #ifdef CONFIG_DIAG_OVER_USB
@@ -104,29 +104,49 @@
 	ret = scnprintf(buf, DEBUG_BUF_SIZE,
 		"Pending status for work_stucts:\n"
 		"diag_drain_work: %d\n"
-		"diag_read_smd_work: %d\n"
-		"diag_read_smd_cntl_work: %d\n"
-		"diag_read_smd_lpass_work: %d\n"
-		"diag_read_smd_lpass_cntl_work: %d\n"
-		"diag_read_smd_wcnss_work: %d\n"
-		"diag_read_smd_wcnss_cntl_work: %d\n"
-		"diag_modem_mask_update_work: %d\n"
-		"diag_lpass_mask_update_work: %d\n"
-		"diag_wcnss_mask_update_work: %d\n"
-		"diag_read_smd_dci_work: %d\n"
-		"diag_update_smd_dci_work: %d\n",
+		"Modem data diag_read_smd_work: %d\n"
+		"LPASS data diag_read_smd_work: %d\n"
+		"RIVA data diag_read_smd_work: %d\n"
+		"Modem cntl diag_read_smd_work: %d\n"
+		"LPASS cntl diag_read_smd_work: %d\n"
+		"RIVA cntl diag_read_smd_work: %d\n"
+		"Modem dci diag_read_smd_work: %d\n"
+		"Modem data diag_notify_update_smd_work: %d\n"
+		"LPASS data diag_notify_update_smd_work: %d\n"
+		"RIVA data diag_notify_update_smd_work: %d\n"
+		"Modem cntl diag_notify_update_smd_work: %d\n"
+		"LPASS cntl diag_notify_update_smd_work: %d\n"
+		"RIVA cntl diag_notify_update_smd_work: %d\n"
+		"Modem dci diag_notify_update_smd_work: %d\n",
 		work_pending(&(driver->diag_drain_work)),
-		work_pending(&(driver->diag_read_smd_work)),
-		work_pending(&(driver->diag_read_smd_cntl_work)),
-		work_pending(&(driver->diag_read_smd_lpass_work)),
-		work_pending(&(driver->diag_read_smd_lpass_cntl_work)),
-		work_pending(&(driver->diag_read_smd_wcnss_work)),
-		work_pending(&(driver->diag_read_smd_wcnss_cntl_work)),
-		work_pending(&(driver->diag_modem_mask_update_work)),
-		work_pending(&(driver->diag_lpass_mask_update_work)),
-		work_pending(&(driver->diag_wcnss_mask_update_work)),
-		work_pending(&(driver->diag_read_smd_dci_work)),
-		work_pending(&(driver->diag_update_smd_dci_work)));
+		work_pending(&(driver->smd_data[MODEM_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_data[LPASS_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_data[WCNSS_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_cntl[MODEM_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_cntl[LPASS_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_cntl[WCNSS_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_dci[MODEM_DATA].
+							diag_read_smd_work)),
+		work_pending(&(driver->smd_data[MODEM_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_data[LPASS_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_data[WCNSS_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_cntl[MODEM_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_cntl[LPASS_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_cntl[WCNSS_DATA].
+						diag_notify_update_smd_work)),
+		work_pending(&(driver->smd_dci[MODEM_DATA].
+						diag_notify_update_smd_work)));
 
 #ifdef CONFIG_DIAG_OVER_USB
 	ret += scnprintf(buf+ret, DEBUG_BUF_SIZE,
@@ -165,6 +185,15 @@
 	}
 
 	bytes_remaining = buf_size;
+
+	if (diag_dbgfs_table_index == 0) {
+		bytes_written = scnprintf(buf+bytes_in_buffer, bytes_remaining,
+			"Client ids: Modem: %d, LPASS: %d, "
+			"WCNSS: %d, APPS: %d\n",
+			MODEM_DATA, LPASS_DATA, WCNSS_DATA, APPS_DATA);
+		bytes_in_buffer += bytes_written;
+	}
+
 	for (i = diag_dbgfs_table_index; i < diag_max_reg; i++) {
 		/* Do not process empty entries in the table */
 		if (driver->table[i].process_id == 0)
@@ -189,7 +218,7 @@
 		if (bytes_remaining < bytes_written)
 			break;
 	}
-	diag_dbgfs_table_index = i;
+	diag_dbgfs_table_index = i+1;
 
 	*ppos = 0;
 	ret = simple_read_from_buffer(ubuf, count, ppos, buf, bytes_in_buffer);
@@ -230,10 +259,10 @@
 		"diag_read_hsic_work: %d\n"
 		"diag_disconnect_work: %d\n"
 		"diag_usb_read_complete_work: %d\n"
-		"smux ch: %d"
-		"smux enabled %d"
-		"smux in busy %d"
-		"smux connected %d",
+		"smux ch: %d\n"
+		"smux enabled %d\n"
+		"smux in busy %d\n"
+		"smux connected %d\n",
 		driver->hsic_ch,
 		driver->hsic_inited,
 		driver->hsic_device_enabled,
diff --git a/drivers/char/diag/diag_masks.c b/drivers/char/diag/diag_masks.c
index 5316548..2a85a00 100644
--- a/drivers/char/diag/diag_masks.c
+++ b/drivers/char/diag/diag_masks.c
@@ -294,29 +294,23 @@
 	mutex_unlock(&driver->diagchar_mutex);
 }
 
-void diag_modem_mask_update_fn(struct work_struct *work)
+void diag_mask_update_fn(struct work_struct *work)
 {
-	diag_send_msg_mask_update(driver->ch_cntl, ALL_SSID,
-					   ALL_SSID, MODEM_PROC);
-	diag_send_log_mask_update(driver->ch_cntl, ALL_EQUIP_ID);
-	diag_send_event_mask_update(driver->ch_cntl, diag_event_num_bytes);
-}
+	struct diag_smd_info *smd_info = container_of(work,
+						struct diag_smd_info,
+						diag_notify_update_smd_work);
+	if (!smd_info) {
+		pr_err("diag: In %s, smd info is null, cannot update masks for the peripheral\n",
+			__func__);
+		return;
+	}
 
-void diag_lpass_mask_update_fn(struct work_struct *work)
-{
-	diag_send_msg_mask_update(driver->chlpass_cntl, ALL_SSID,
-						   ALL_SSID, LPASS_PROC);
-	diag_send_log_mask_update(driver->chlpass_cntl, ALL_EQUIP_ID);
-	diag_send_event_mask_update(driver->chlpass_cntl, diag_event_num_bytes);
-}
+	diag_send_msg_mask_update(smd_info->ch, ALL_SSID, ALL_SSID,
+						smd_info->peripheral);
+	diag_send_log_mask_update(smd_info->ch, ALL_EQUIP_ID);
+	diag_send_event_mask_update(smd_info->ch, diag_event_num_bytes);
 
-void diag_wcnss_mask_update_fn(struct work_struct *work)
-{
-	diag_send_msg_mask_update(driver->ch_wcnss_cntl, ALL_SSID,
-						   ALL_SSID, WCNSS_PROC);
-	diag_send_log_mask_update(driver->ch_wcnss_cntl, ALL_EQUIP_ID);
-	diag_send_event_mask_update(driver->ch_wcnss_cntl,
-						 diag_event_num_bytes);
+	smd_info->notify_context = 0;
 }
 
 void diag_send_log_mask_update(smd_channel_t *ch, int equip_id)
@@ -493,15 +487,13 @@
 			payload_length = 8 + ((*(int *)(buf + 4)) + 7)/8;
 			for (i = 0; i < payload_length; i++)
 				*(int *)(driver->apps_rsp_buf+12+i) = *(buf+i);
-			if (driver->ch_cntl)
-				diag_send_log_mask_update(driver->ch_cntl,
-				*(int *)buf);
-			if (driver->chlpass_cntl)
-				diag_send_log_mask_update(driver->chlpass_cntl,
-				*(int *)buf);
-			if (driver->ch_wcnss_cntl)
-				diag_send_log_mask_update(driver->ch_wcnss_cntl,
-				*(int *)buf);
+
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_log_mask_update(
+						driver->smd_cntl[i].ch,
+						*(int *)buf);
+			}
 			encode_rsp_and_send(12 + payload_length - 1);
 			return 0;
 		}
@@ -509,7 +501,8 @@
 	} /* Get log masks */
 	else if (*buf == 0x73 && *(int *)(buf+4) == 4) {
 #if defined(CONFIG_DIAG_OVER_USB)
-		if (!(driver->ch) && chk_apps_only()) {
+		if (!(driver->smd_data[MODEM_DATA].ch) &&
+						chk_apps_only()) {
 			equip_id = *(int *)(buf + 8);
 			num_items = *(int *)(buf + 12);
 			driver->apps_rsp_buf[0] = 0x73;
@@ -541,15 +534,13 @@
 			driver->apps_rsp_buf[3] = 0x0;
 			*(int *)(driver->apps_rsp_buf + 4) = 0x0;
 			*(int *)(driver->apps_rsp_buf + 8) = 0x0; /* status */
-			if (driver->ch_cntl)
-				diag_send_log_mask_update(driver->ch_cntl,
-				ALL_EQUIP_ID);
-			if (driver->chlpass_cntl)
-				diag_send_log_mask_update(driver->chlpass_cntl,
-				ALL_EQUIP_ID);
-			if (driver->ch_wcnss_cntl)
-				diag_send_log_mask_update(driver->ch_wcnss_cntl,
-				ALL_EQUIP_ID);
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_log_mask_update(
+						driver->smd_cntl[i].ch,
+						ALL_EQUIP_ID);
+
+			}
 			encode_rsp_and_send(11);
 			return 0;
 		}
@@ -559,7 +550,8 @@
 		ssid_first = *(uint16_t *)(buf + 2);
 		ssid_last = *(uint16_t *)(buf + 4);
 #if defined(CONFIG_DIAG_OVER_USB)
-		if (!(driver->ch) && chk_apps_only()) {
+		if (!(driver->smd_data[MODEM_DATA].ch) &&
+						chk_apps_only()) {
 			driver->apps_rsp_buf[0] = 0x7d;
 			driver->apps_rsp_buf[1] = 0x3;
 			*(uint16_t *)(driver->apps_rsp_buf+2) = ssid_first;
@@ -599,15 +591,14 @@
 			for (i = 0; i < 8 + ssid_range; i++)
 				*(driver->apps_rsp_buf + i) = *(buf+i);
 			*(driver->apps_rsp_buf + 6) = 0x1;
-			if (driver->ch_cntl)
-				diag_send_msg_mask_update(driver->ch_cntl,
-				ssid_first, ssid_last, MODEM_PROC);
-			if (driver->chlpass_cntl)
-				diag_send_msg_mask_update(driver->chlpass_cntl,
-				ssid_first, ssid_last, LPASS_PROC);
-			if (driver->ch_wcnss_cntl)
-				diag_send_msg_mask_update(driver->ch_wcnss_cntl,
-				ssid_first, ssid_last, WCNSS_PROC);
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_msg_mask_update(
+						driver->smd_cntl[i].ch,
+						ssid_first, ssid_last,
+						driver->smd_cntl[i].peripheral);
+
+			}
 			encode_rsp_and_send(8 + ssid_range - 1);
 			return 0;
 		}
@@ -625,15 +616,14 @@
 			driver->apps_rsp_buf[3] = 0; /* rsvd */
 			*(int *)(driver->apps_rsp_buf + 4) = rt_mask;
 			/* send msg mask update to peripheral */
-			if (driver->ch_cntl)
-				diag_send_msg_mask_update(driver->ch_cntl,
-				ALL_SSID, ALL_SSID, MODEM_PROC);
-			if (driver->chlpass_cntl)
-				diag_send_msg_mask_update(driver->chlpass_cntl,
-				ALL_SSID, ALL_SSID, LPASS_PROC);
-			if (driver->ch_wcnss_cntl)
-				diag_send_msg_mask_update(driver->ch_wcnss_cntl,
-				ALL_SSID, ALL_SSID, WCNSS_PROC);
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_msg_mask_update(
+						driver->smd_cntl[i].ch,
+						ALL_SSID, ALL_SSID,
+						driver->smd_cntl[i].peripheral);
+
+			}
 			encode_rsp_and_send(7);
 			return 0;
 		}
@@ -652,16 +642,12 @@
 				EVENT_LAST_ID + 1;
 			memcpy(driver->apps_rsp_buf+6, driver->event_masks,
 				EVENT_LAST_ID/8+1);
-			if (driver->ch_cntl)
-				diag_send_event_mask_update(driver->ch_cntl,
-				diag_event_num_bytes);
-			if (driver->chlpass_cntl)
-				diag_send_event_mask_update(
-				driver->chlpass_cntl,
-				diag_event_num_bytes);
-			if (driver->ch_wcnss_cntl)
-				diag_send_event_mask_update(
-				driver->ch_wcnss_cntl, diag_event_num_bytes);
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_event_mask_update(
+						driver->smd_cntl[i].ch,
+						diag_event_num_bytes);
+			}
 			encode_rsp_and_send(6 + EVENT_LAST_ID/8);
 			return 0;
 		}
@@ -675,16 +661,12 @@
 			driver->apps_rsp_buf[0] = 0x60;
 			driver->apps_rsp_buf[1] = 0x0;
 			driver->apps_rsp_buf[2] = 0x0;
-			if (driver->ch_cntl)
-				diag_send_event_mask_update(driver->ch_cntl,
-				diag_event_num_bytes);
-			if (driver->chlpass_cntl)
-				diag_send_event_mask_update(
-				driver->chlpass_cntl,
-				diag_event_num_bytes);
-			if (driver->ch_wcnss_cntl)
-				diag_send_event_mask_update(
-				driver->ch_wcnss_cntl, diag_event_num_bytes);
+			for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
+				if (driver->smd_cntl[i].ch)
+					diag_send_event_mask_update(
+						driver->smd_cntl[i].ch,
+						diag_event_num_bytes);
+			}
 			encode_rsp_and_send(2);
 			return 0;
 		}
@@ -760,14 +742,6 @@
 			goto err;
 		kmemleak_not_leak(driver->event_masks);
 	}
-#ifdef CONFIG_DIAG_OVER_USB
-	INIT_WORK(&(driver->diag_modem_mask_update_work),
-						 diag_modem_mask_update_fn);
-	INIT_WORK(&(driver->diag_lpass_mask_update_work),
-						 diag_lpass_mask_update_fn);
-	INIT_WORK(&(driver->diag_wcnss_mask_update_work),
-						 diag_wcnss_mask_update_fn);
-#endif
 	return;
 err:
 	pr_err("diag: Could not initialize diag mask buffers");
diff --git a/drivers/char/diag/diag_masks.h b/drivers/char/diag/diag_masks.h
index bcf5bc2..f144774 100644
--- a/drivers/char/diag/diag_masks.h
+++ b/drivers/char/diag/diag_masks.h
@@ -20,6 +20,7 @@
 void diag_send_msg_mask_update(smd_channel_t *, int ssid_first,
 					 int ssid_last, int proc);
 void diag_send_log_mask_update(smd_channel_t *, int);
+void diag_mask_update_fn(struct work_struct *work);
 int diag_process_apps_masks(unsigned char *buf, int len);
 void diag_masks_init(void);
 void diag_masks_exit(void);
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index ec3bb81..77f9dd6 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -23,6 +23,7 @@
 #include <mach/msm_smd.h>
 #include <asm/atomic.h>
 #include <asm/mach-types.h>
+
 /* Size of the USB buffers used for read and write*/
 #define USB_MAX_OUT_BUF 4096
 #define APPS_BUF_SIZE	2000
@@ -40,17 +41,14 @@
 #define POOL_TYPE_HSIC		8
 #define POOL_TYPE_HSIC_WRITE	16
 #define POOL_TYPE_ALL		7
-#define MODEM_DATA		1
-#define LPASS_DATA		2
+#define MODEM_DATA		0
+#define LPASS_DATA		1
+#define WCNSS_DATA		2
 #define APPS_DATA		3
 #define SDIO_DATA		4
-#define WCNSS_DATA		5
-#define HSIC_DATA		6
-#define SMUX_DATA		7
-#define MODEM_PROC		0
+#define HSIC_DATA		5
+#define SMUX_DATA		6
 #define APPS_PROC		1
-#define LPASS_PROC		2
-#define WCNSS_PROC		3
 #define MSG_MASK_SIZE 10000
 #define LOG_MASK_SIZE 8000
 #define EVENT_MASK_SIZE 1000
@@ -76,6 +74,14 @@
 #define DIAG_STATUS_OPEN (0x00010000)	/* DCI channel open status mask   */
 #define DIAG_STATUS_CLOSED (0x00020000)	/* DCI channel closed status mask */
 
+#define NUM_SMD_DATA_CHANNELS 3
+#define NUM_SMD_CONTROL_CHANNELS 3
+#define NUM_SMD_DCI_CHANNELS 1
+
+#define SMD_DATA_TYPE 0
+#define SMD_CNTL_TYPE 1
+#define SMD_DCI_TYPE 2
+
 /* Maximum number of pkt reg supported at initialization*/
 extern unsigned int diag_max_reg;
 extern unsigned int diag_threshold_reg;
@@ -137,6 +143,35 @@
 };
 #endif
 
+struct diag_smd_info {
+	int peripheral;	/* The peripheral this smd channel communicates with */
+	int type;	/* The type of smd channel (data, control, dci) */
+	uint16_t peripheral_mask;
+
+	smd_channel_t *ch;
+	smd_channel_t *ch_save;
+
+	int in_busy_1;
+	int in_busy_2;
+
+	unsigned char *buf_in_1;
+	unsigned char *buf_in_2;
+
+	struct diag_request *write_ptr_1;
+	struct diag_request *write_ptr_2;
+
+	struct work_struct diag_read_smd_work;
+	struct work_struct diag_notify_update_smd_work;
+	int notify_context;
+
+	/*
+	 * Function ptr for function to call to process the data that
+	 * was just read from the smd channel
+	 */
+	int (*process_smd_read_data)(struct diag_smd_info *smd_info,
+						void *buf, int num_bytes);
+};
+
 struct diagchar_dev {
 
 	/* State for the char driver */
@@ -190,16 +225,9 @@
 	struct diag_ctrl_log_mask *log_mask;
 	struct diag_ctrl_msg_mask *msg_mask;
 	/* State for diag forwarding */
-	unsigned char *buf_in_1;
-	unsigned char *buf_in_2;
-	unsigned char *buf_in_cntl;
-	unsigned char *buf_in_lpass_1;
-	unsigned char *buf_in_lpass_2;
-	unsigned char *buf_in_lpass_cntl;
-	unsigned char *buf_in_wcnss_1;
-	unsigned char *buf_in_wcnss_2;
-	unsigned char *buf_in_wcnss_cntl;
-	unsigned char *buf_in_dci;
+	struct diag_smd_info smd_data[NUM_SMD_DATA_CHANNELS];
+	struct diag_smd_info smd_cntl[NUM_SMD_CONTROL_CHANNELS];
+	struct diag_smd_info smd_dci[NUM_SMD_DCI_CHANNELS];
 	unsigned char *usb_buf_out;
 	unsigned char *apps_rsp_buf;
 	unsigned char *user_space_data;
@@ -207,20 +235,6 @@
 	unsigned char *buf_msg_mask_update;
 	unsigned char *buf_log_mask_update;
 	unsigned char *buf_event_mask_update;
-	smd_channel_t *ch;
-	smd_channel_t *ch_cntl;
-	smd_channel_t *ch_dci;
-	smd_channel_t *chlpass;
-	smd_channel_t *chlpass_cntl;
-	smd_channel_t *ch_wcnss;
-	smd_channel_t *ch_wcnss_cntl;
-	int in_busy_1;
-	int in_busy_2;
-	int in_busy_lpass_1;
-	int in_busy_lpass_2;
-	int in_busy_wcnss_1;
-	int in_busy_wcnss_2;
-	int in_busy_dci;
 	int read_len_legacy;
 	unsigned char *hdlc_buf;
 	unsigned hdlc_count;
@@ -233,21 +247,7 @@
 #endif
 	struct workqueue_struct *diag_wq;
 	struct work_struct diag_drain_work;
-	struct work_struct diag_read_smd_work;
-	struct work_struct diag_read_smd_cntl_work;
-	struct work_struct diag_read_smd_lpass_work;
-	struct work_struct diag_read_smd_lpass_cntl_work;
-	struct work_struct diag_read_smd_wcnss_work;
-	struct work_struct diag_read_smd_wcnss_cntl_work;
 	struct workqueue_struct *diag_cntl_wq;
-	struct work_struct diag_modem_mask_update_work;
-	struct work_struct diag_lpass_mask_update_work;
-	struct work_struct diag_wcnss_mask_update_work;
-	struct work_struct diag_read_smd_dci_work;
-	struct work_struct diag_update_smd_dci_work;
-	struct work_struct diag_clean_modem_reg_work;
-	struct work_struct diag_clean_lpass_reg_work;
-	struct work_struct diag_clean_wcnss_reg_work;
 	uint8_t *msg_masks;
 	uint8_t *log_masks;
 	int log_masks_length;
@@ -255,15 +255,8 @@
 	struct diag_master_table *table;
 	uint8_t *pkt_buf;
 	int pkt_length;
-	struct diag_request *write_ptr_1;
-	struct diag_request *write_ptr_2;
 	struct diag_request *usb_read_ptr;
 	struct diag_request *write_ptr_svc;
-	struct diag_request *write_ptr_lpass_1;
-	struct diag_request *write_ptr_lpass_2;
-	struct diag_request *write_ptr_wcnss_1;
-	struct diag_request *write_ptr_wcnss_2;
-	struct diag_write_device *write_ptr_dci;
 	int logging_mode;
 	int mask_check;
 	int logging_process_id;
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index 34ff345..9bd8950 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -149,21 +149,6 @@
 void diag_clear_hsic_tbl(void) { }
 #endif
 
-void diag_read_smd_work_fn(struct work_struct *work)
-{
-	__diag_smd_send_req();
-}
-
-void diag_read_smd_lpass_work_fn(struct work_struct *work)
-{
-	__diag_smd_lpass_send_req();
-}
-
-void diag_read_smd_wcnss_work_fn(struct work_struct *work)
-{
-	__diag_smd_wcnss_send_req();
-}
-
 void diag_add_client(int i, struct file *file)
 {
 	struct diagchar_priv *diagpriv_data;
@@ -336,7 +321,7 @@
 	return 0;
 }
 
-void diag_clear_reg(int proc_num)
+void diag_clear_reg(int peripheral)
 {
 	int i;
 
@@ -344,9 +329,8 @@
 	/* reset polling flag */
 	driver->polling_reg_flag = 0;
 	for (i = 0; i < diag_max_reg; i++) {
-		if (driver->table[i].client_id == proc_num) {
+		if (driver->table[i].client_id == peripheral)
 			driver->table[i].process_id = 0;
-		}
 	}
 	/* re-scan the registration table */
 	for (i = 0; i < diag_max_reg; i++) {
@@ -373,7 +357,7 @@
 			driver->polling_reg_flag = 1;
 	if (params->proc_id == APPS_PROC) {
 		driver->table[j].process_id = current->tgid;
-		driver->table[j].client_id = APPS_PROC;
+		driver->table[j].client_id = APPS_DATA;
 	} else {
 		driver->table[j].process_id = NON_APPS_PROC;
 		driver->table[j].client_id = params->client_id;
@@ -565,9 +549,11 @@
 			return -EFAULT;
 		mutex_lock(&driver->dci_mutex);
 		if (!(driver->num_dci_client))
-			driver->in_busy_dci = 0;
+			for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
+				driver->smd_dci[i].in_busy_1 = 0;
 		driver->num_dci_client++;
-		pr_debug("diag: id = %d\n", driver->dci_client_id);
+		pr_debug("diag: In %s, id = %d\n",
+				__func__, driver->dci_client_id);
 		driver->dci_client_id++;
 		for (i = 0; i < MAX_DCI_CLIENTS; i++) {
 			if (driver->dci_client_tbl[i].client == NULL) {
@@ -616,8 +602,11 @@
 		mutex_unlock(&driver->dci_mutex);
 		return success;
 	} else if (iocmd == DIAG_IOCTL_DCI_SUPPORT) {
-		if (driver->ch_dci)
-			support_list = support_list | DIAG_CON_MPSS;
+		for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
+			if (driver->smd_dci[i].ch)
+				support_list |=
+				driver->smd_dci[i].peripheral_mask;
+		}
 		if (copy_to_user((void *)ioarg, &support_list,
 							 sizeof(uint16_t)))
 			return -EFAULT;
@@ -700,12 +689,10 @@
 		mutex_unlock(&driver->diagchar_mutex);
 		if (temp == MEMORY_DEVICE_MODE && driver->logging_mode
 							== NO_LOGGING_MODE) {
-			driver->in_busy_1 = 1;
-			driver->in_busy_2 = 1;
-			driver->in_busy_lpass_1 = 1;
-			driver->in_busy_lpass_2 = 1;
-			driver->in_busy_wcnss_1 = 1;
-			driver->in_busy_wcnss_2 = 1;
+			for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+				driver->smd_data[i].in_busy_1 = 0;
+				driver->smd_data[i].in_busy_2 = 0;
+			}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 			driver->in_busy_sdio = 1;
 #endif
@@ -715,22 +702,15 @@
 #endif
 		} else if (temp == NO_LOGGING_MODE && driver->logging_mode
 							== MEMORY_DEVICE_MODE) {
-			driver->in_busy_1 = 0;
-			driver->in_busy_2 = 0;
-			driver->in_busy_lpass_1 = 0;
-			driver->in_busy_lpass_2 = 0;
-			driver->in_busy_wcnss_1 = 0;
-			driver->in_busy_wcnss_2 = 0;
-			/* Poll SMD channels to check for data*/
-			if (driver->ch)
-				queue_work(driver->diag_wq,
-					&(driver->diag_read_smd_work));
-			if (driver->chlpass)
-				queue_work(driver->diag_wq,
-					&(driver->diag_read_smd_lpass_work));
-			if (driver->ch_wcnss)
-				queue_work(driver->diag_wq,
-					&(driver->diag_read_smd_wcnss_work));
+			for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+				driver->smd_data[i].in_busy_1 = 0;
+				driver->smd_data[i].in_busy_2 = 0;
+				/* Poll SMD channels to check for data*/
+				if (driver->smd_data[i].ch)
+					queue_work(driver->diag_wq,
+						&(driver->smd_data[i].
+							diag_read_smd_work));
+			}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 			driver->in_busy_sdio = 0;
 			/* Poll SDIO channel to check for data */
@@ -758,23 +738,15 @@
 		} else if (temp == USB_MODE && driver->logging_mode
 							== MEMORY_DEVICE_MODE) {
 			diagfwd_disconnect();
-			driver->in_busy_1 = 0;
-			driver->in_busy_2 = 0;
-			driver->in_busy_lpass_1 = 0;
-			driver->in_busy_lpass_2 = 0;
-			driver->in_busy_wcnss_1 = 0;
-			driver->in_busy_wcnss_2 = 0;
-
-			/* Poll SMD channels to check for data*/
-			if (driver->ch)
-				queue_work(driver->diag_wq,
-					 &(driver->diag_read_smd_work));
-			if (driver->chlpass)
-				queue_work(driver->diag_wq,
-					&(driver->diag_read_smd_lpass_work));
-			if (driver->ch_wcnss)
-				queue_work(driver->diag_wq,
-					&(driver->diag_read_smd_wcnss_work));
+			for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+				driver->smd_data[i].in_busy_1 = 0;
+				driver->smd_data[i].in_busy_2 = 0;
+				/* Poll SMD channels to check for data*/
+				if (driver->smd_data[i].ch)
+					queue_work(driver->diag_wq,
+						&(driver->smd_data[i].
+							diag_read_smd_work));
+			}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 			driver->in_busy_sdio = 0;
 			/* Poll SDIO channel to check for data */
@@ -886,73 +858,30 @@
 		}
 
 		/* copy modem data */
-		if (driver->in_busy_1 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-					 (driver->write_ptr_1->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-					*(driver->buf_in_1),
-					 driver->write_ptr_1->length);
-			driver->in_busy_1 = 0;
-		}
-		if (driver->in_busy_2 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-					 (driver->write_ptr_2->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-					 *(driver->buf_in_2),
-					 driver->write_ptr_2->length);
-			driver->in_busy_2 = 0;
-		}
-		/* copy lpass data */
-		if (driver->in_busy_lpass_1 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-				 (driver->write_ptr_lpass_1->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver->
-							buf_in_lpass_1),
-					 driver->write_ptr_lpass_1->length);
-			driver->in_busy_lpass_1 = 0;
-		}
-		if (driver->in_busy_lpass_2 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-				 (driver->write_ptr_lpass_2->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver->
-				buf_in_lpass_2), driver->
-					write_ptr_lpass_2->length);
-			driver->in_busy_lpass_2 = 0;
-		}
-		/* copy wncss data */
-		if (driver->in_busy_wcnss_1 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-				 (driver->write_ptr_wcnss_1->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver->
-							buf_in_wcnss_1),
-					 driver->write_ptr_wcnss_1->length);
-			driver->in_busy_wcnss_1 = 0;
-		}
-		if (driver->in_busy_wcnss_2 == 1) {
-			num_data++;
-			/*Copy the length of data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret,
-				 (driver->write_ptr_wcnss_2->length), 4);
-			/*Copy the actual data being passed*/
-			COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver->
-							buf_in_wcnss_2),
-					 driver->write_ptr_wcnss_2->length);
-			driver->in_busy_wcnss_2 = 0;
+		for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+			struct diag_smd_info *data = &driver->smd_data[i];
+			if (data->in_busy_1 == 1) {
+				num_data++;
+				/*Copy the length of data being passed*/
+				COPY_USER_SPACE_OR_EXIT(buf+ret,
+					(data->write_ptr_1->length), 4);
+				/*Copy the actual data being passed*/
+				COPY_USER_SPACE_OR_EXIT(buf+ret,
+					*(data->buf_in_1),
+					data->write_ptr_1->length);
+				data->in_busy_1 = 0;
+			}
+			if (data->in_busy_2 == 1) {
+				num_data++;
+				/*Copy the length of data being passed*/
+				COPY_USER_SPACE_OR_EXIT(buf+ret,
+					(data->write_ptr_2->length), 4);
+				/*Copy the actual data being passed*/
+				COPY_USER_SPACE_OR_EXIT(buf+ret,
+					*(data->buf_in_2),
+					data->write_ptr_2->length);
+				data->in_busy_2 = 0;
+			}
 		}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 		/* copy 9K data over SDIO */
@@ -1045,15 +974,11 @@
 		COPY_USER_SPACE_OR_EXIT(buf+4, num_data, 4);
 		ret -= 4;
 		driver->data_ready[index] ^= USER_SPACE_DATA_TYPE;
-		if (driver->ch)
-			queue_work(driver->diag_wq,
-					 &(driver->diag_read_smd_work));
-		if (driver->chlpass)
-			queue_work(driver->diag_wq,
-					 &(driver->diag_read_smd_lpass_work));
-		if (driver->ch_wcnss)
-			queue_work(driver->diag_wq,
-					 &(driver->diag_read_smd_wcnss_work));
+		for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+			if (driver->smd_data[i].ch)
+				queue_work(driver->diag_wq,
+				&(driver->smd_data[i].diag_read_smd_work));
+		}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 		if (driver->sdio_ch)
 			queue_work(driver->diag_sdio_wq,
@@ -1135,10 +1060,12 @@
 			}
 		}
 		driver->data_ready[index] ^= DCI_DATA_TYPE;
-		driver->in_busy_dci = 0;
-		if (driver->ch_dci)
-			queue_work(driver->diag_dci_wq,
-				&(driver->diag_read_smd_dci_work));
+		for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
+			driver->smd_dci[i].in_busy_1 = 0;
+			if (driver->smd_dci[i].ch)
+				queue_work(driver->diag_dci_wq,
+				&(driver->smd_dci[i].diag_read_smd_work));
+		}
 		goto exit;
 	}
 exit:
@@ -1601,27 +1528,6 @@
 		init_waitqueue_head(&driver->wait_q);
 		init_waitqueue_head(&driver->smd_wait_q);
 		INIT_WORK(&(driver->diag_drain_work), diag_drain_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_work), diag_read_smd_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_cntl_work),
-						 diag_read_smd_cntl_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_lpass_work),
-			   diag_read_smd_lpass_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_lpass_cntl_work),
-			   diag_read_smd_lpass_cntl_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_wcnss_work),
-			diag_read_smd_wcnss_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_wcnss_cntl_work),
-			diag_read_smd_wcnss_cntl_work_fn);
-		INIT_WORK(&(driver->diag_read_smd_dci_work),
-						diag_read_smd_dci_work_fn);
-		INIT_WORK(&(driver->diag_update_smd_dci_work),
-						diag_update_smd_dci_work_fn);
-		INIT_WORK(&(driver->diag_clean_modem_reg_work),
-						 diag_clean_modem_reg_fn);
-		INIT_WORK(&(driver->diag_clean_lpass_reg_work),
-						 diag_clean_lpass_reg_fn);
-		INIT_WORK(&(driver->diag_clean_wcnss_reg_work),
-						 diag_clean_wcnss_reg_fn);
 		diag_debugfs_init();
 		diag_masks_init();
 		diagfwd_init();
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index cee4c96..6b4c337 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -49,26 +49,26 @@
 unsigned char diag_debug_buf[1024];
 static unsigned int buf_tbl_size = 8; /*Number of entries in table of buffers */
 struct diag_master_table entry;
-smd_channel_t *ch_temp = NULL, *chlpass_temp = NULL, *ch_wcnss_temp = NULL;
 struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
 struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
 
 void encode_rsp_and_send(int buf_length)
 {
+	struct diag_smd_info *data = &(driver->smd_data[MODEM_DATA]);
 	send.state = DIAG_STATE_START;
 	send.pkt = driver->apps_rsp_buf;
 	send.last = (void *)(driver->apps_rsp_buf + buf_length);
 	send.terminate = 1;
-	if (!driver->in_busy_1) {
-		enc.dest = driver->buf_in_1;
-		enc.dest_last = (void *)(driver->buf_in_1 + APPS_BUF_SIZE - 1);
+	if (!data->in_busy_1) {
+		enc.dest = data->buf_in_1;
+		enc.dest_last = (void *)(data->buf_in_1 + APPS_BUF_SIZE - 1);
 		diag_hdlc_encode(&send, &enc);
-		driver->write_ptr_1->buf = driver->buf_in_1;
-		driver->write_ptr_1->length = (int)(enc.dest -
-						(void *)(driver->buf_in_1));
-		driver->in_busy_1 = 1;
-		diag_device_write(driver->buf_in_1, MODEM_DATA,
-					driver->write_ptr_1);
+		data->write_ptr_1->buf = data->buf_in_1;
+		data->write_ptr_1->length = (int)(enc.dest -
+						(void *)(data->buf_in_1));
+		data->in_busy_1 = 1;
+		diag_device_write(data->buf_in_1, data->peripheral,
+					data->write_ptr_1);
 		memset(driver->apps_rsp_buf, '\0', APPS_BUF_SIZE);
 	}
 }
@@ -178,7 +178,8 @@
 		 * has registered to respond for polling
 		 */
 		return 1;
-	else if (!(driver->ch) && !(chk_apps_master()))
+	else if (!(driver->smd_data[MODEM_DATA].ch) &&
+					!(chk_apps_master()))
 		/*
 		 * If the apps processor is not the master and the modem
 		 * is not up
@@ -221,39 +222,66 @@
 	}
 }
 
-void __diag_smd_send_req(void)
+/* Process the data read from the smd data channel */
+int diag_process_smd_read_data(struct diag_smd_info *smd_info, void *buf,
+								int total_recd)
 {
-	void *buf = NULL, *temp_buf = NULL;
-	int total_recd = 0, r = 0, pkt_len, *in_busy_ptr = NULL;
-	int loop_count = 0;
 	struct diag_request *write_ptr_modem = NULL;
+	int *in_busy_ptr = 0;
 
-	if (!driver->in_busy_1) {
-		buf = driver->buf_in_1;
-		write_ptr_modem = driver->write_ptr_1;
-		in_busy_ptr = &(driver->in_busy_1);
-	} else if (!driver->in_busy_2) {
-		buf = driver->buf_in_2;
-		write_ptr_modem = driver->write_ptr_2;
-		in_busy_ptr = &(driver->in_busy_2);
+	if (smd_info->buf_in_1 == buf) {
+		write_ptr_modem = smd_info->write_ptr_1;
+		in_busy_ptr = &smd_info->in_busy_1;
+	} else if (smd_info->buf_in_2 == buf) {
+		write_ptr_modem = smd_info->write_ptr_2;
+		in_busy_ptr = &smd_info->in_busy_2;
+	} else {
+		pr_err("diag: In %s, no match for in_busy_1\n", __func__);
 	}
 
-	if (driver->ch && buf) {
+	if (write_ptr_modem) {
+		write_ptr_modem->length = total_recd;
+		*in_busy_ptr = 1;
+		diag_device_write(buf, smd_info->peripheral, write_ptr_modem);
+	}
+
+	return 0;
+}
+
+void diag_smd_send_req(struct diag_smd_info *smd_info)
+{
+	void *buf = NULL, *temp_buf = NULL;
+	int total_recd = 0, r = 0, pkt_len;
+	int loop_count = 0;
+	int notify = 0;
+
+	if (!smd_info) {
+		pr_err("diag: In %s, no smd info. Not able to read.\n",
+			__func__);
+		return;
+	}
+
+	if (!smd_info->in_busy_1)
+		buf = smd_info->buf_in_1;
+	else if ((smd_info->type == SMD_DATA_TYPE) && !smd_info->in_busy_2)
+		buf = smd_info->buf_in_2;
+
+	if (smd_info->ch && buf) {
 		temp_buf = buf;
-		pkt_len = smd_cur_packet_size(driver->ch);
+		pkt_len = smd_cur_packet_size(smd_info->ch);
 
 		while (pkt_len && (pkt_len != total_recd)) {
 			loop_count++;
-			r = smd_read_avail(driver->ch);
+			r = smd_read_avail(smd_info->ch);
 			pr_debug("diag: In %s, received pkt %d %d\n",
 				__func__, r, total_recd);
 			if (!r) {
 				/* Nothing to read from SMD */
 				wait_event(driver->smd_wait_q,
-					((driver->ch == 0) ||
-					smd_read_avail(driver->ch)));
+					((smd_info->ch == 0) ||
+					smd_read_avail(smd_info->ch)));
 				/* If the smd channel is open */
-				if (driver->ch) {
+				if (smd_info->ch) {
 					pr_debug("diag: In %s, return from wait_event\n",
 						__func__);
 					continue;
@@ -269,7 +297,7 @@
 					pr_err("diag: In %s, SMD sending in packets up to %d bytes\n",
 						__func__, total_recd);
 					buf = krealloc(buf, total_recd,
-							GFP_KERNEL);
+						GFP_KERNEL);
 				} else {
 					pr_err("diag: In %s, SMD sending in packets more than %d bytes\n",
 						__func__, MAX_IN_BUF_SIZE);
@@ -281,38 +309,49 @@
 					__func__);
 				return;
 			}
-			if (pkt_len > r)
-				pr_debug("diag: In %s, SMD sending partial pkt %d %d %d %d\n",
-					__func__, pkt_len, r, total_recd,
-					loop_count);
+			if (pkt_len > r) {
+				pr_err("diag: In %s, SMD sending partial pkt %d %d %d %d %d %d\n",
+				__func__, pkt_len, r, total_recd, loop_count,
+				smd_info->peripheral, smd_info->type);
+			}
+
 			/* keep reading for complete packet */
-			smd_read(driver->ch, temp_buf, r);
+			smd_read(smd_info->ch, temp_buf, r);
 			temp_buf += r;
 		}
 
 		if (total_recd > 0) {
-			if (!buf)
+			if (!buf) {
 				pr_err("diag: Out of diagmem for Modem\n");
-			else {
-				APPEND_DEBUG('j');
-				write_ptr_modem->length = total_recd;
-				*in_busy_ptr = 1;
-				diag_device_write(buf, MODEM_DATA,
-							 write_ptr_modem);
+			} else if (smd_info->process_smd_read_data) {
+				notify = smd_info->process_smd_read_data(
+						smd_info, buf, total_recd);
+				/* Poll SMD channels to check for data */
+				if (notify)
+					diag_smd_notify(smd_info,
+							SMD_EVENT_DATA);
 			}
 		}
-	} else if (driver->ch && !buf &&
+	} else if (smd_info->ch && !buf &&
 		(driver->logging_mode == MEMORY_DEVICE_MODE)) {
-		chk_logging_wakeup();
+			chk_logging_wakeup();
 	}
 }
 
-int diag_device_write(void *buf, int proc_num, struct diag_request *write_ptr)
+void diag_read_smd_work_fn(struct work_struct *work)
+{
+	struct diag_smd_info *smd_info = container_of(work,
+							struct diag_smd_info,
+							diag_read_smd_work);
+	diag_smd_send_req(smd_info);
+}
+
+int diag_device_write(void *buf, int data_type, struct diag_request *write_ptr)
 {
 	int i, err = 0;
 
 	if (driver->logging_mode == MEMORY_DEVICE_MODE) {
-		if (proc_num == APPS_DATA) {
+		if (data_type == APPS_DATA) {
 			for (i = 0; i < driver->poolsize_write_struct; i++)
 				if (driver->buf_tbl[i].length == 0) {
 					driver->buf_tbl[i].buf = buf;
@@ -329,7 +368,7 @@
 		}
 
 #ifdef CONFIG_DIAGFWD_BRIDGE_CODE
-		else if (proc_num == HSIC_DATA) {
+		else if (data_type == HSIC_DATA) {
 			unsigned long flags;
 			int foundIndex = -1;
 
@@ -364,31 +403,22 @@
 		} else
 			return -EINVAL;
 	} else if (driver->logging_mode == NO_LOGGING_MODE) {
-		if (proc_num == MODEM_DATA) {
-			driver->in_busy_1 = 0;
-			driver->in_busy_2 = 0;
-			queue_work(driver->diag_wq, &(driver->
+		if ((data_type >= 0) && (data_type < NUM_SMD_DATA_CHANNELS)) {
+			driver->smd_data[data_type].in_busy_1 = 0;
+			driver->smd_data[data_type].in_busy_2 = 0;
+			queue_work(driver->diag_wq,
+				&(driver->smd_data[data_type].
 							diag_read_smd_work));
-		} else if (proc_num == LPASS_DATA) {
-			driver->in_busy_lpass_1 = 0;
-			driver->in_busy_lpass_2 = 0;
-			queue_work(driver->diag_wq, &(driver->
-						diag_read_smd_lpass_work));
-		}  else if (proc_num == WCNSS_DATA) {
-			driver->in_busy_wcnss_1 = 0;
-			driver->in_busy_wcnss_2 = 0;
-			queue_work(driver->diag_wq, &(driver->
-				diag_read_smd_wcnss_work));
 		}
 #ifdef CONFIG_DIAG_SDIO_PIPE
-		else if (proc_num == SDIO_DATA) {
+		else if (data_type == SDIO_DATA) {
 			driver->in_busy_sdio = 0;
 			queue_work(driver->diag_sdio_wq,
 				&(driver->diag_read_sdio_work));
 		}
 #endif
 #ifdef CONFIG_DIAGFWD_BRIDGE_CODE
-		else if (proc_num == HSIC_DATA) {
+		else if (data_type == HSIC_DATA) {
 			if (driver->hsic_ch)
 				queue_work(diag_bridge[HSIC].wq,
 					&(driver->diag_read_hsic_work));
@@ -398,7 +428,7 @@
 	}
 #ifdef CONFIG_DIAG_OVER_USB
 	else if (driver->logging_mode == USB_MODE) {
-		if (proc_num == APPS_DATA) {
+		if (data_type == APPS_DATA) {
 			driver->write_ptr_svc = (struct diag_request *)
 			(diagmem_alloc(driver, sizeof(struct diag_request),
 				 POOL_TYPE_WRITE_STRUCT));
@@ -409,7 +439,8 @@
 						driver->write_ptr_svc);
 			} else
 				err = -1;
-		} else if (proc_num == MODEM_DATA) {
+		} else if ((data_type >= 0) &&
+				(data_type < NUM_SMD_DATA_CHANNELS)) {
 			write_ptr->buf = buf;
 #ifdef DIAG_DEBUG
 			printk(KERN_INFO "writing data to USB,"
@@ -419,15 +450,9 @@
 					    buf, write_ptr->length, 1);
 #endif /* DIAG DEBUG */
 			err = usb_diag_write(driver->legacy_ch, write_ptr);
-		} else if (proc_num == LPASS_DATA) {
-			write_ptr->buf = buf;
-			err = usb_diag_write(driver->legacy_ch, write_ptr);
-		} else if (proc_num == WCNSS_DATA) {
-			write_ptr->buf = buf;
-			err = usb_diag_write(driver->legacy_ch, write_ptr);
 		}
 #ifdef CONFIG_DIAG_SDIO_PIPE
-		else if (proc_num == SDIO_DATA) {
+		else if (data_type == SDIO_DATA) {
 			if (machine_is_msm8x60_fusion() ||
 					 machine_is_msm8x60_fusn_ffa()) {
 				write_ptr->buf = buf;
@@ -438,7 +463,7 @@
 		}
 #endif
 #ifdef CONFIG_DIAGFWD_BRIDGE_CODE
-		else if (proc_num == HSIC_DATA) {
+		else if (data_type == HSIC_DATA) {
 			if (driver->hsic_device_enabled) {
 				struct diag_request *write_ptr_mdm;
 				write_ptr_mdm = (struct diag_request *)
@@ -469,7 +494,7 @@
 						"while USB write\n");
 				err = -1;
 			}
-		} else if (proc_num == SMUX_DATA) {
+		} else if (data_type == SMUX_DATA) {
 				write_ptr->buf = buf;
 				write_ptr->context = (void *)SMUX;
 				pr_debug("diag: writing SMUX data\n");
@@ -483,179 +508,6 @@
     return err;
 }
 
-void __diag_smd_wcnss_send_req(void)
-{
-	void *buf = NULL, *temp_buf = NULL;
-	int total_recd = 0, r = 0, pkt_len, *in_busy_wcnss_ptr = NULL;
-	struct diag_request *write_ptr_wcnss = NULL;
-	int loop_count = 0;
-
-	if (!driver->in_busy_wcnss_1) {
-		buf = driver->buf_in_wcnss_1;
-		write_ptr_wcnss = driver->write_ptr_wcnss_1;
-		in_busy_wcnss_ptr = &(driver->in_busy_wcnss_1);
-	} else if (!driver->in_busy_wcnss_2) {
-		buf = driver->buf_in_wcnss_2;
-		write_ptr_wcnss = driver->write_ptr_wcnss_2;
-		in_busy_wcnss_ptr = &(driver->in_busy_wcnss_2);
-	}
-
-	if (driver->ch_wcnss && buf) {
-		temp_buf = buf;
-		pkt_len = smd_cur_packet_size(driver->ch_wcnss);
-
-		while (pkt_len && (pkt_len != total_recd)) {
-			loop_count++;
-			r = smd_read_avail(driver->ch_wcnss);
-			pr_debug("diag: In %s, received pkt %d %d\n",
-				__func__, r, total_recd);
-			if (!r) {
-				/* Nothing to read from SMD */
-				wait_event(driver->smd_wait_q,
-					((driver->ch_wcnss == 0) ||
-					smd_read_avail(driver->ch_wcnss)));
-				/* If the smd channel is open */
-				if (driver->ch_wcnss) {
-					pr_debug("diag: In %s, return from wait_event\n",
-						__func__);
-					continue;
-				} else {
-					pr_debug("diag: In %s, return from wait_event ch_wcnss closed\n",
-						__func__);
-					return;
-				}
-			}
-			total_recd += r;
-			if (total_recd > IN_BUF_SIZE) {
-				if (total_recd < MAX_IN_BUF_SIZE) {
-					pr_err("diag: In %s, SMD sending in packets up to %d bytes\n",
-						__func__, total_recd);
-					buf = krealloc(buf, total_recd,
-								 GFP_KERNEL);
-				} else {
-					pr_err("diag: In %s, SMD sending in packets more than %d bytes\n",
-						__func__, MAX_IN_BUF_SIZE);
-					return;
-				}
-			}
-			if (pkt_len < r) {
-				pr_err("diag: In %s, SMD sending incorrect pkt\n",
-					__func__);
-				return;
-			}
-			if (pkt_len > r) {
-				pr_debug("diag: In %s, SMD sending partial pkt %d %d %d %d\n",
-					__func__, pkt_len, r, total_recd,
-					loop_count);
-			}
-			/* keep reading for complete packet */
-			smd_read(driver->ch_wcnss, temp_buf, r);
-			temp_buf += r;
-		}
-
-		if (total_recd > 0) {
-			if (!buf) {
-				pr_err("diag: Out of diagmem for wcnss\n");
-			} else {
-				APPEND_DEBUG('j');
-				write_ptr_wcnss->length = total_recd;
-				*in_busy_wcnss_ptr = 1;
-				diag_device_write(buf, WCNSS_DATA,
-					 write_ptr_wcnss);
-			}
-		}
-	} else if (driver->ch_wcnss && !buf &&
-		(driver->logging_mode == MEMORY_DEVICE_MODE)) {
-		chk_logging_wakeup();
-	}
-}
-
-void __diag_smd_lpass_send_req(void)
-{
-	void *buf = NULL, *temp_buf = NULL;
-	int total_recd = 0, r = 0, pkt_len, *in_busy_lpass_ptr = NULL;
-	struct diag_request *write_ptr_lpass = NULL;
-	int loop_count = 0;
-
-	if (!driver->in_busy_lpass_1) {
-		buf = driver->buf_in_lpass_1;
-		write_ptr_lpass = driver->write_ptr_lpass_1;
-		in_busy_lpass_ptr = &(driver->in_busy_lpass_1);
-	} else if (!driver->in_busy_lpass_2) {
-		buf = driver->buf_in_lpass_2;
-		write_ptr_lpass = driver->write_ptr_lpass_2;
-		in_busy_lpass_ptr = &(driver->in_busy_lpass_2);
-	}
-
-	if (driver->chlpass && buf) {
-		temp_buf = buf;
-		pkt_len = smd_cur_packet_size(driver->chlpass);
-
-		while (pkt_len && (pkt_len != total_recd)) {
-			loop_count++;
-			r = smd_read_avail(driver->chlpass);
-			pr_debug("diag: In %s, received pkt %d %d\n",
-				__func__, r, total_recd);
-			if (!r) {
-				/* Nothing to read from SMD */
-				wait_event(driver->smd_wait_q,
-					((driver->chlpass == 0) ||
-					smd_read_avail(driver->chlpass)));
-				/* If the smd channel is open */
-				if (driver->chlpass) {
-					pr_debug("diag: In %s, return from wait_event\n",
-						__func__);
-					continue;
-				} else {
-					pr_debug("diag: In %s, return from wait_event chlpass closed\n",
-						__func__);
-					return;
-				}
-			}
-			total_recd += r;
-			if (total_recd > IN_BUF_SIZE) {
-				if (total_recd < MAX_IN_BUF_SIZE) {
-					pr_err("diag: In %s, SMD sending in packets up to %d bytes\n",
-						__func__, total_recd);
-					buf = krealloc(buf, total_recd,
-								 GFP_KERNEL);
-				} else {
-					pr_err("diag: In %s, SMD sending in packets more than %d bytes\n",
-						__func__, MAX_IN_BUF_SIZE);
-					return;
-				}
-			}
-			if (pkt_len < r) {
-				pr_err("diag: In %s, SMD sending incorrect pkt\n",
-					__func__);
-				return;
-			}
-			if (pkt_len > r)
-				pr_debug("diag: In %s, SMD sending partial pkt %d %d %d %d\n",
-					__func__, pkt_len, r, total_recd,
-					loop_count);
-			/* keep reading for complete packet */
-			smd_read(driver->chlpass, temp_buf, r);
-			temp_buf += r;
-		}
-
-		if (total_recd > 0) {
-			if (!buf)
-				pr_err("diag: Out of diagmem for LPASS\n");
-			else {
-				APPEND_DEBUG('j');
-				write_ptr_lpass->length = total_recd;
-				*in_busy_lpass_ptr = 1;
-				diag_device_write(buf, LPASS_DATA,
-							 write_ptr_lpass);
-			}
-		}
-	} else if (driver->chlpass && !buf &&
-		(driver->logging_mode == MEMORY_DEVICE_MODE)) {
-		chk_logging_wakeup();
-	}
-}
-
 static void diag_update_pkt_buffer(unsigned char *buf)
 {
 	unsigned char *ptr = driver->pkt_buf;
@@ -695,6 +547,15 @@
 	mutex_unlock(&driver->diagchar_mutex);
 }
 
+static int diag_check_mode_reset(unsigned char *buf)
+{
+	int is_mode_reset = 0;
+	if (chk_apps_master() && (int)(*(char *)buf) == MODE_CMD)
+		if ((int)(*(char *)(buf+1)) == RESET_ID)
+			is_mode_reset = 1;
+	return is_mode_reset;
+}
+
 void diag_send_data(struct diag_master_table entry, unsigned char *buf,
 					 int len, int type)
 {
@@ -704,21 +565,23 @@
 		diag_update_sleeping_process(entry.process_id, PKT_TYPE);
 	} else {
 		if (len > 0) {
-			if (entry.client_id == MODEM_PROC && driver->ch) {
-				if (chk_apps_master() &&
-					 (int)(*(char *)buf) == MODE_CMD)
-					if ((int)(*(char *)(buf+1)) ==
-						RESET_ID)
+			if ((entry.client_id >= 0) &&
+				(entry.client_id < NUM_SMD_DATA_CHANNELS)) {
+				int index = entry.client_id;
+				if (driver->smd_data[index].ch) {
+					if ((index == MODEM_DATA) &&
+						diag_check_mode_reset(buf)) {
 						return;
-				smd_write(driver->ch, buf, len);
-			} else if (entry.client_id == LPASS_PROC &&
-							 driver->chlpass) {
-				smd_write(driver->chlpass, buf, len);
-			} else if (entry.client_id == WCNSS_PROC &&
-							 driver->ch_wcnss) {
-				smd_write(driver->ch_wcnss, buf, len);
+					}
+					smd_write(driver->smd_data[index].ch,
+							buf, len);
+				} else {
+					pr_err("diag: In %s, smd channel %d not open\n",
+						__func__, index);
+				}
 			} else {
-				pr_alert("diag: incorrect channel");
+				pr_alert("diag: In %s, incorrect channel: %d",
+					__func__, entry.client_id);
 			}
 		}
 	}
@@ -799,7 +662,8 @@
 		return 0;
 	}
 	/* Check for Apps Only & get event mask request */
-	else if (!(driver->ch) && chk_apps_only() && *buf == 0x81) {
+	else if (!(driver->smd_data[MODEM_DATA].ch) && chk_apps_only() &&
+								*buf == 0x81) {
 		driver->apps_rsp_buf[0] = 0x81;
 		driver->apps_rsp_buf[1] = 0x0;
 		*(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0;
@@ -810,7 +674,7 @@
 		return 0;
 	}
 	/* Get log ID range & Check for Apps Only */
-	else if (!(driver->ch) && chk_apps_only()
+	else if (!(driver->smd_data[MODEM_DATA].ch) && chk_apps_only()
 			  && (*buf == 0x73) && *(int *)(buf+4) == 1) {
 		driver->apps_rsp_buf[0] = 0x73;
 		*(int *)(driver->apps_rsp_buf + 4) = 0x1; /* operation ID */
@@ -835,7 +699,7 @@
 		return 0;
 	}
 	/* Respond to Get SSID Range request message */
-	else if (!(driver->ch) && chk_apps_only()
+	else if (!(driver->smd_data[MODEM_DATA].ch) && chk_apps_only()
 			 && (*buf == 0x7d) && (*(buf+1) == 0x1)) {
 		driver->apps_rsp_buf[0] = 0x7d;
 		driver->apps_rsp_buf[1] = 0x1;
@@ -893,7 +757,7 @@
 		return 0;
 	}
 	/* Check for Apps Only Respond to Get Subsys Build mask */
-	else if (!(driver->ch) && chk_apps_only()
+	else if (!(driver->smd_data[MODEM_DATA].ch) && chk_apps_only()
 			 && (*buf == 0x7d) && (*(buf+1) == 0x2)) {
 		ssid_first = *(uint16_t *)(buf + 2);
 		ssid_last = *(uint16_t *)(buf + 4);
@@ -1110,13 +974,14 @@
 		type = 0;
 	}
 	/* implies this packet is NOT meant for apps */
-	if (!(driver->ch) && type == 1) {
+	if (!(driver->smd_data[MODEM_DATA].ch) && type == 1) {
 		if (chk_apps_only()) {
 			diag_send_error_rsp(hdlc.dest_idx);
 		} else { /* APQ 8060, Let Q6 respond */
-			if (driver->chlpass)
-				smd_write(driver->chlpass, driver->hdlc_buf,
-						  hdlc.dest_idx - 3);
+			if (driver->smd_data[LPASS_DATA].ch)
+				smd_write(driver->smd_data[LPASS_DATA].ch,
+						driver->hdlc_buf,
+						hdlc.dest_idx - 3);
 		}
 		type = 0;
 	}
@@ -1128,9 +993,11 @@
 							driver->hdlc_buf)+i));
 #endif /* DIAG DEBUG */
 	/* ignore 2 bytes for CRC, one for 7E and send */
-	if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) {
+	if ((driver->smd_data[MODEM_DATA].ch) && (ret) && (type) &&
+						(hdlc.dest_idx > 3)) {
 		APPEND_DEBUG('g');
-		smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3);
+		smd_write(driver->smd_data[MODEM_DATA].ch,
+					driver->hdlc_buf, hdlc.dest_idx - 3);
 		APPEND_DEBUG('h');
 #ifdef DIAG_DEBUG
 		printk(KERN_INFO "writing data to SMD, pkt length %d\n", len);
@@ -1148,6 +1015,7 @@
 int diagfwd_connect(void)
 {
 	int err;
+	int i;
 
 	printk(KERN_DEBUG "diag: USB connected\n");
 	err = usb_diag_alloc_req(driver->legacy_ch, N_LEGACY_WRITE,
@@ -1156,21 +1024,16 @@
 		printk(KERN_ERR "diag: unable to alloc USB req on legacy ch");
 
 	driver->usb_connected = 1;
-	driver->in_busy_1 = 0;
-	driver->in_busy_2 = 0;
-	driver->in_busy_lpass_1 = 0;
-	driver->in_busy_lpass_2 = 0;
-	driver->in_busy_wcnss_1 = 0;
-	driver->in_busy_wcnss_2 = 0;
+	for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+		driver->smd_data[i].in_busy_1 = 0;
+		driver->smd_data[i].in_busy_2 = 0;
+		/* Poll SMD data channels to check for data */
+		queue_work(driver->diag_wq,
+			&(driver->smd_data[i].diag_read_smd_work));
+		/* Poll SMD CNTL channels to check for data */
+		diag_smd_notify(&(driver->smd_cntl[i]), SMD_EVENT_DATA);
+	}
 
-	/* Poll SMD channels to check for data*/
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_lpass_work));
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_work));
-	/* Poll SMD CNTL channels to check for data */
-	diag_smd_cntl_notify(NULL, SMD_EVENT_DATA);
-	diag_smd_lpass_cntl_notify(NULL, SMD_EVENT_DATA);
-	diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA);
 	/* Poll USB channel to check for data*/
 	queue_work(driver->diag_wq, &(driver->diag_read_work));
 #ifdef CONFIG_DIAG_SDIO_PIPE
@@ -1186,17 +1049,17 @@
 
 int diagfwd_disconnect(void)
 {
+	int i;
+
 	printk(KERN_DEBUG "diag: USB disconnected\n");
 	driver->usb_connected = 0;
 	driver->debug_flag = 1;
 	usb_diag_free_req(driver->legacy_ch);
 	if (driver->logging_mode == USB_MODE) {
-		driver->in_busy_1 = 1;
-		driver->in_busy_2 = 1;
-		driver->in_busy_lpass_1 = 1;
-		driver->in_busy_lpass_2 = 1;
-		driver->in_busy_wcnss_1 = 1;
-		driver->in_busy_wcnss_2 = 1;
+		for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+			driver->smd_data[i].in_busy_1 = 1;
+			driver->smd_data[i].in_busy_2 = 1;
+		}
 	}
 #ifdef CONFIG_DIAG_SDIO_PIPE
 	if (machine_is_msm8x60_fusion() || machine_is_msm8x60_fusn_ffa())
@@ -1210,50 +1073,44 @@
 int diagfwd_write_complete(struct diag_request *diag_write_ptr)
 {
 	unsigned char *buf = diag_write_ptr->buf;
-	/*Determine if the write complete is for data from modem/apps/q6 */
+	int found_it = 0;
+	int i;
+
+	/* Determine if the write complete is for data from modem/apps/q6 */
 	/* Need a context variable here instead */
-	if (buf == (void *)driver->buf_in_1) {
-		driver->in_busy_1 = 0;
-		APPEND_DEBUG('o');
-		queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
-	} else if (buf == (void *)driver->buf_in_2) {
-		driver->in_busy_2 = 0;
-		APPEND_DEBUG('O');
-		queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
-	} else if (buf == (void *)driver->buf_in_lpass_1) {
-		driver->in_busy_lpass_1 = 0;
-		APPEND_DEBUG('p');
-		queue_work(driver->diag_wq,
-				&(driver->diag_read_smd_lpass_work));
-	} else if (buf == (void *)driver->buf_in_lpass_2) {
-		driver->in_busy_lpass_2 = 0;
-		APPEND_DEBUG('P');
-		queue_work(driver->diag_wq,
-				&(driver->diag_read_smd_lpass_work));
-	} else if (buf == driver->buf_in_wcnss_1) {
-		driver->in_busy_wcnss_1 = 0;
-		APPEND_DEBUG('r');
-		queue_work(driver->diag_wq,
-			 &(driver->diag_read_smd_wcnss_work));
-	} else if (buf == driver->buf_in_wcnss_2) {
-		driver->in_busy_wcnss_2 = 0;
-		APPEND_DEBUG('R');
-		queue_work(driver->diag_wq,
-			 &(driver->diag_read_smd_wcnss_work));
+	for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++) {
+		struct diag_smd_info *data = &(driver->smd_data[i]);
+		if (buf == (void *)data->buf_in_1) {
+			data->in_busy_1 = 0;
+			queue_work(driver->diag_wq,
+				&(data->diag_read_smd_work));
+			found_it = 1;
+			break;
+		} else if (buf == (void *)data->buf_in_2) {
+			data->in_busy_2 = 0;
+			queue_work(driver->diag_wq,
+				&(data->diag_read_smd_work));
+			found_it = 1;
+			break;
+		}
 	}
 #ifdef CONFIG_DIAG_SDIO_PIPE
-	else if (buf == (void *)driver->buf_in_sdio)
-		if (machine_is_msm8x60_fusion() ||
-			 machine_is_msm8x60_fusn_ffa())
-			diagfwd_write_complete_sdio();
-		else
-			pr_err("diag: Incorrect buffer pointer while WRITE");
+	if (!found_it) {
+		if (buf == (void *)driver->buf_in_sdio) {
+			if (machine_is_msm8x60_fusion() ||
+				 machine_is_msm8x60_fusn_ffa())
+				diagfwd_write_complete_sdio();
+			else
+				pr_err("diag: Incorrect buffer pointer while WRITE");
+			found_it = 1;
+		}
+	}
 #endif
-	else {
-		diagmem_free(driver, (unsigned char *)buf, POOL_TYPE_HDLC);
+	if (!found_it) {
+		diagmem_free(driver, (unsigned char *)buf,
+						POOL_TYPE_HDLC);
 		diagmem_free(driver, (unsigned char *)diag_write_ptr,
-						 POOL_TYPE_WRITE_STRUCT);
-		APPEND_DEBUG('q');
+						POOL_TYPE_WRITE_STRUCT);
 	}
 	return 0;
 }
@@ -1340,76 +1197,87 @@
 
 #endif /* DIAG OVER USB */
 
-static void diag_smd_notify(void *ctxt, unsigned event)
+void diag_smd_notify(void *ctxt, unsigned event)
 {
-	if (event == SMD_EVENT_CLOSE) {
-		driver->ch = 0;
-		wake_up(&driver->smd_wait_q);
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_clean_modem_reg_work));
+	struct diag_smd_info *smd_info = (struct diag_smd_info *)ctxt;
+	if (!smd_info)
 		return;
-	} else if (event == SMD_EVENT_OPEN) {
-		if (ch_temp)
-			driver->ch = ch_temp;
-	}
-	wake_up(&driver->smd_wait_q);
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_work));
-}
 
-#if defined(CONFIG_MSM_N_WAY_SMD)
-static void diag_smd_lpass_notify(void *ctxt, unsigned event)
-{
 	if (event == SMD_EVENT_CLOSE) {
-		driver->chlpass = 0;
+		smd_info->ch = 0;
 		wake_up(&driver->smd_wait_q);
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_clean_lpass_reg_work));
+		if (smd_info->type == SMD_DATA_TYPE) {
+			smd_info->notify_context = event;
+			queue_work(driver->diag_cntl_wq,
+				 &(smd_info->diag_notify_update_smd_work));
+		} else if (smd_info->type == SMD_DCI_TYPE) {
+			/* Notify the clients of the close */
+			diag_dci_notify_client(smd_info->peripheral_mask,
+							DIAG_STATUS_CLOSED);
+		}
 		return;
 	} else if (event == SMD_EVENT_OPEN) {
-		if (chlpass_temp)
-			driver->chlpass = chlpass_temp;
-	}
-	wake_up(&driver->smd_wait_q);
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_lpass_work));
-}
-#endif
+		if (smd_info->ch_save)
+			smd_info->ch = smd_info->ch_save;
 
-static void diag_smd_wcnss_notify(void *ctxt, unsigned event)
-{
-	if (event == SMD_EVENT_CLOSE) {
-		driver->ch_wcnss = 0;
-		wake_up(&driver->smd_wait_q);
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_clean_wcnss_reg_work));
-		return;
-	} else if (event == SMD_EVENT_OPEN) {
-		if (ch_wcnss_temp)
-			driver->ch_wcnss = ch_wcnss_temp;
+		if (smd_info->type == SMD_CNTL_TYPE) {
+			smd_info->notify_context = event;
+			queue_work(driver->diag_cntl_wq,
+				&(smd_info->diag_notify_update_smd_work));
+		} else if (smd_info->type == SMD_DCI_TYPE) {
+			smd_info->notify_context = event;
+			queue_work(driver->diag_dci_wq,
+				&(smd_info->diag_notify_update_smd_work));
+			/* Notify the clients of the open */
+			diag_dci_notify_client(smd_info->peripheral_mask,
+							DIAG_STATUS_OPEN);
+		}
 	}
+
 	wake_up(&driver->smd_wait_q);
-	queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_work));
+
+	if (smd_info->type == SMD_DCI_TYPE)
+		queue_work(driver->diag_dci_wq,
+				&(smd_info->diag_read_smd_work));
+	else
+		queue_work(driver->diag_wq, &(smd_info->diag_read_smd_work));
 }
 
 static int diag_smd_probe(struct platform_device *pdev)
 {
 	int r = 0;
+	int index = -1;
 
 	if (pdev->id == SMD_APPS_MODEM) {
-		r = smd_open("DIAG", &driver->ch, driver, diag_smd_notify);
-		ch_temp = driver->ch;
+		index = MODEM_DATA;
+		r = smd_open("DIAG", &driver->smd_data[index].ch,
+					&driver->smd_data[index],
+					diag_smd_notify);
+		driver->smd_data[index].ch_save =
+					driver->smd_data[index].ch;
 	}
 #if defined(CONFIG_MSM_N_WAY_SMD)
 	if (pdev->id == SMD_APPS_QDSP) {
+		index = LPASS_DATA;
 		r = smd_named_open_on_edge("DIAG", SMD_APPS_QDSP,
-			&driver->chlpass, driver, diag_smd_lpass_notify);
-		chlpass_temp = driver->chlpass;
+					&driver->smd_data[index].ch,
+					&driver->smd_data[index],
+					diag_smd_notify);
+		driver->smd_data[index].ch_save =
+					driver->smd_data[index].ch;
 	}
 #endif
 	if (pdev->id == SMD_APPS_WCNSS) {
-		r = smd_named_open_on_edge("APPS_RIVA_DATA", SMD_APPS_WCNSS
-			, &driver->ch_wcnss, driver, diag_smd_wcnss_notify);
-		ch_wcnss_temp = driver->ch_wcnss;
+		index = WCNSS_DATA;
+		r = smd_named_open_on_edge("APPS_RIVA_DATA",
+					SMD_APPS_WCNSS,
+					&driver->smd_data[index].ch,
+					&driver->smd_data[index],
+					diag_smd_notify);
+		driver->smd_data[index].ch_save =
+					driver->smd_data[index].ch;
 	}
+
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 	pr_debug("diag: open SMD port, Id = %d, r = %d\n", pdev->id, r);
@@ -1417,86 +1285,190 @@
 	return 0;
 }
 
-static int diagfwd_runtime_suspend(struct device *dev)
+static int diag_smd_runtime_suspend(struct device *dev)
 {
 	dev_dbg(dev, "pm_runtime: suspending...\n");
 	return 0;
 }
 
-static int diagfwd_runtime_resume(struct device *dev)
+static int diag_smd_runtime_resume(struct device *dev)
 {
 	dev_dbg(dev, "pm_runtime: resuming...\n");
 	return 0;
 }
 
-static const struct dev_pm_ops diagfwd_dev_pm_ops = {
-	.runtime_suspend = diagfwd_runtime_suspend,
-	.runtime_resume = diagfwd_runtime_resume,
+static const struct dev_pm_ops diag_smd_dev_pm_ops = {
+	.runtime_suspend = diag_smd_runtime_suspend,
+	.runtime_resume = diag_smd_runtime_resume,
 };
 
 static struct platform_driver msm_smd_ch1_driver = {
 
 	.probe = diag_smd_probe,
 	.driver = {
-		   .name = "DIAG",
-		   .owner = THIS_MODULE,
-		   .pm   = &diagfwd_dev_pm_ops,
-		   },
+		.name = "DIAG",
+		.owner = THIS_MODULE,
+		.pm   = &diag_smd_dev_pm_ops,
+	},
 };
 
 static struct platform_driver diag_smd_lite_driver = {
 
 	.probe = diag_smd_probe,
 	.driver = {
-		   .name = "APPS_RIVA_DATA",
-		   .owner = THIS_MODULE,
-		   .pm   = &diagfwd_dev_pm_ops,
-		   },
+		.name = "APPS_RIVA_DATA",
+		.owner = THIS_MODULE,
+		.pm   = &diag_smd_dev_pm_ops,
+	},
 };
 
+void diag_smd_destructor(struct diag_smd_info *smd_info)
+{
+	if (smd_info->ch)
+		smd_close(smd_info->ch);
+
+	smd_info->ch = 0;
+	smd_info->ch_save = 0;
+	kfree(smd_info->buf_in_1);
+	kfree(smd_info->buf_in_2);
+	kfree(smd_info->write_ptr_1);
+	kfree(smd_info->write_ptr_2);
+}
+
+int diag_smd_constructor(struct diag_smd_info *smd_info, int peripheral,
+			  int type)
+{
+	smd_info->peripheral = peripheral;
+	smd_info->type = type;
+
+	switch (peripheral) {
+	case MODEM_DATA:
+		smd_info->peripheral_mask = DIAG_CON_MPSS;
+		break;
+	case LPASS_DATA:
+		smd_info->peripheral_mask = DIAG_CON_LPASS;
+		break;
+	case WCNSS_DATA:
+		smd_info->peripheral_mask = DIAG_CON_WCNSS;
+		break;
+	default:
+		pr_err("diag: In %s, unknown peripheral, peripheral: %d\n",
+			__func__, peripheral);
+		goto err;
+	}
+
+	smd_info->ch = 0;
+	smd_info->ch_save = 0;
+
+	if (smd_info->buf_in_1 == NULL) {
+		smd_info->buf_in_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
+		if (smd_info->buf_in_1 == NULL)
+			goto err;
+		kmemleak_not_leak(smd_info->buf_in_1);
+	}
+
+	if (smd_info->write_ptr_1 == NULL) {
+		smd_info->write_ptr_1 = kzalloc(sizeof(struct diag_request),
+								GFP_KERNEL);
+		if (smd_info->write_ptr_1 == NULL)
+			goto err;
+		kmemleak_not_leak(smd_info->write_ptr_1);
+	}
+
+	/* The smd data type needs two buffers */
+	if (smd_info->type == SMD_DATA_TYPE) {
+		if (smd_info->buf_in_2 == NULL) {
+			smd_info->buf_in_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
+			if (smd_info->buf_in_2 == NULL)
+				goto err;
+			kmemleak_not_leak(smd_info->buf_in_2);
+		}
+		if (smd_info->write_ptr_2 == NULL) {
+			smd_info->write_ptr_2 =
+				kzalloc(sizeof(struct diag_request),
+				GFP_KERNEL);
+			if (smd_info->write_ptr_2 == NULL)
+				goto err;
+			kmemleak_not_leak(smd_info->write_ptr_2);
+		}
+	}
+
+	INIT_WORK(&(smd_info->diag_read_smd_work), diag_read_smd_work_fn);
+
+	/*
+	 * The update function assigned to the diag_notify_update_smd_work
+	 * work_struct is meant to be used for updating that is not to
+	 * be done in the context of the smd notify function. The
+	 * notify_context variable can be used for passing additional
+	 * information to the update function.
+	 */
+	smd_info->notify_context = 0;
+	if (type == SMD_DATA_TYPE)
+		INIT_WORK(&(smd_info->diag_notify_update_smd_work),
+							diag_clean_reg_fn);
+	else if (type == SMD_CNTL_TYPE)
+		INIT_WORK(&(smd_info->diag_notify_update_smd_work),
+							diag_mask_update_fn);
+	else if (type == SMD_DCI_TYPE)
+		INIT_WORK(&(smd_info->diag_notify_update_smd_work),
+						diag_update_smd_dci_work_fn);
+	else {
+		pr_err("diag: In %s, unknown type, type: %d\n", __func__, type);
+		goto err;
+	}
+
+	/*
+	 * Set function ptr for function to call to process the data that
+	 * was just read from the smd channel
+	 */
+	if (type == SMD_DATA_TYPE)
+		smd_info->process_smd_read_data = diag_process_smd_read_data;
+	else if (type == SMD_CNTL_TYPE)
+		smd_info->process_smd_read_data =
+						diag_process_smd_cntl_read_data;
+	else if (type == SMD_DCI_TYPE)
+		smd_info->process_smd_read_data =
+						diag_process_smd_dci_read_data;
+	else {
+		pr_err("diag: In %s, unknown type, type: %d\n", __func__, type);
+		goto err;
+	}
+
+	return 1;
+err:
+	kfree(smd_info->buf_in_1);
+	kfree(smd_info->buf_in_2);
+	kfree(smd_info->write_ptr_1);
+	kfree(smd_info->write_ptr_2);
+
+	return 0;
+}
+
 void diagfwd_init(void)
 {
+	int success;
+	int i;
+
 	diag_debug_buf_idx = 0;
 	driver->read_len_legacy = 0;
 	driver->use_device_tree = has_device_tree();
 	mutex_init(&driver->diag_cntl_mutex);
 
-	if (driver->buf_in_1 == NULL) {
-		driver->buf_in_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_1);
-	}
-	if (driver->buf_in_2 == NULL) {
-		driver->buf_in_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_2);
-	}
-	if (driver->buf_in_lpass_1 == NULL) {
-		driver->buf_in_lpass_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_lpass_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_lpass_1);
-	}
-	if (driver->buf_in_lpass_2 == NULL) {
-		driver->buf_in_lpass_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_lpass_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_lpass_2);
-	}
-	if (driver->buf_in_wcnss_1 == NULL) {
-		driver->buf_in_wcnss_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_wcnss_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_wcnss_1);
-	}
-	if (driver->buf_in_wcnss_2 == NULL) {
-		driver->buf_in_wcnss_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_wcnss_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_wcnss_2);
-	}
+	success = diag_smd_constructor(&driver->smd_data[MODEM_DATA],
+					MODEM_DATA, SMD_DATA_TYPE);
+	if (!success)
+		goto err;
+
+	success = diag_smd_constructor(&driver->smd_data[LPASS_DATA],
+					LPASS_DATA, SMD_DATA_TYPE);
+	if (!success)
+		goto err;
+
+	success = diag_smd_constructor(&driver->smd_data[WCNSS_DATA],
+					WCNSS_DATA, SMD_DATA_TYPE);
+	if (!success)
+		goto err;
+
 	if (driver->usb_buf_out  == NULL &&
 	     (driver->usb_buf_out = kzalloc(USB_MAX_OUT_BUF,
 					 GFP_KERNEL)) == NULL)
@@ -1534,48 +1506,6 @@
 		       GFP_KERNEL)) == NULL)
 		goto err;
 	kmemleak_not_leak(driver->table);
-	if (driver->write_ptr_1 == NULL) {
-		driver->write_ptr_1 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_1);
-	}
-	if (driver->write_ptr_2 == NULL) {
-		driver->write_ptr_2 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_2);
-	}
-	if (driver->write_ptr_lpass_1 == NULL) {
-		driver->write_ptr_lpass_1 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_lpass_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_lpass_1);
-	}
-	if (driver->write_ptr_lpass_2 == NULL) {
-		driver->write_ptr_lpass_2 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_lpass_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_lpass_2);
-	}
-	if (driver->write_ptr_wcnss_1 == NULL) {
-		driver->write_ptr_wcnss_1 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_wcnss_1 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_wcnss_1);
-	}
-	if (driver->write_ptr_wcnss_2 == NULL) {
-		driver->write_ptr_wcnss_2 = kzalloc(
-			sizeof(struct diag_request), GFP_KERNEL);
-		if (driver->write_ptr_wcnss_2 == NULL)
-			goto err;
-		kmemleak_not_leak(driver->write_ptr_wcnss_2);
-	}
 
 	if (driver->usb_read_ptr == NULL) {
 		driver->usb_read_ptr = kzalloc(
@@ -1608,16 +1538,13 @@
 #endif
 	platform_driver_register(&msm_smd_ch1_driver);
 	platform_driver_register(&diag_smd_lite_driver);
-
 	return;
 err:
 	pr_err("diag: Could not initialize diag buffers");
-	kfree(driver->buf_in_1);
-	kfree(driver->buf_in_2);
-	kfree(driver->buf_in_lpass_1);
-	kfree(driver->buf_in_lpass_2);
-	kfree(driver->buf_in_wcnss_1);
-	kfree(driver->buf_in_wcnss_2);
+
+	for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_data[i]);
+
 	kfree(driver->buf_msg_mask_update);
 	kfree(driver->buf_log_mask_update);
 	kfree(driver->buf_event_mask_update);
@@ -1628,12 +1555,6 @@
 	kfree(driver->data_ready);
 	kfree(driver->table);
 	kfree(driver->pkt_buf);
-	kfree(driver->write_ptr_1);
-	kfree(driver->write_ptr_2);
-	kfree(driver->write_ptr_lpass_1);
-	kfree(driver->write_ptr_lpass_2);
-	kfree(driver->write_ptr_wcnss_1);
-	kfree(driver->write_ptr_wcnss_2);
 	kfree(driver->usb_read_ptr);
 	kfree(driver->apps_rsp_buf);
 	kfree(driver->user_space_data);
@@ -1643,12 +1564,11 @@
 
 void diagfwd_exit(void)
 {
-	smd_close(driver->ch);
-	smd_close(driver->chlpass);
-	smd_close(driver->ch_wcnss);
-	driver->ch = 0;		/* SMD can make this NULL */
-	driver->chlpass = 0;
-	driver->ch_wcnss = 0;
+	int i;
+
+	for (i = 0; i < NUM_SMD_DATA_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_data[i]);
+
 #ifdef CONFIG_DIAG_OVER_USB
 	if (driver->usb_connected)
 		usb_diag_free_req(driver->legacy_ch);
@@ -1657,12 +1577,7 @@
 	platform_driver_unregister(&msm_smd_ch1_driver);
 	platform_driver_unregister(&msm_diag_dci_driver);
 	platform_driver_unregister(&diag_smd_lite_driver);
-	kfree(driver->buf_in_1);
-	kfree(driver->buf_in_2);
-	kfree(driver->buf_in_lpass_1);
-	kfree(driver->buf_in_lpass_2);
-	kfree(driver->buf_in_wcnss_1);
-	kfree(driver->buf_in_wcnss_2);
+
 	kfree(driver->buf_msg_mask_update);
 	kfree(driver->buf_log_mask_update);
 	kfree(driver->buf_event_mask_update);
@@ -1673,12 +1588,6 @@
 	kfree(driver->data_ready);
 	kfree(driver->table);
 	kfree(driver->pkt_buf);
-	kfree(driver->write_ptr_1);
-	kfree(driver->write_ptr_2);
-	kfree(driver->write_ptr_lpass_1);
-	kfree(driver->write_ptr_lpass_2);
-	kfree(driver->write_ptr_wcnss_1);
-	kfree(driver->write_ptr_wcnss_2);
 	kfree(driver->usb_read_ptr);
 	kfree(driver->apps_rsp_buf);
 	kfree(driver->user_space_data);
diff --git a/drivers/char/diag/diagfwd.h b/drivers/char/diag/diagfwd.h
index a0631d6..14e2dd5 100644
--- a/drivers/char/diag/diagfwd.h
+++ b/drivers/char/diag/diagfwd.h
@@ -22,9 +22,7 @@
 void diagfwd_init(void);
 void diagfwd_exit(void);
 void diag_process_hdlc(void *data, unsigned len);
-void __diag_smd_send_req(void);
-void __diag_smd_lpass_send_req(void);
-void __diag_smd_wcnss_send_req(void);
+void diag_smd_send_req(struct diag_smd_info *smd_info);
 void diag_usb_legacy_notifier(void *, unsigned, struct diag_request *);
 long diagchar_ioctl(struct file *, unsigned int, unsigned long);
 int diag_device_write(void *, int, struct diag_request *);
@@ -37,6 +35,10 @@
 void diag_update_userspace_clients(unsigned int type);
 void diag_update_sleeping_process(int process_id, int data_type);
 void encode_rsp_and_send(int buf_length);
+void diag_smd_notify(void *ctxt, unsigned event);
+int diag_smd_constructor(struct diag_smd_info *smd_info, int peripheral,
+			 int type);
+void diag_smd_destructor(struct diag_smd_info *smd_info);
 /* State for diag forwarding */
 #ifdef CONFIG_DIAG_OVER_USB
 int diagfwd_connect(void);
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index 4848a1d..a900d97 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -21,253 +21,145 @@
 static uint16_t reg_dirty;
 #define HDR_SIZ 8
 
-void diag_clean_modem_reg_fn(struct work_struct *work)
+void diag_clean_reg_fn(struct work_struct *work)
 {
-	pr_debug("diag: clean modem registration\n");
-	reg_dirty |= DIAG_CON_MPSS;
-	diag_clear_reg(MODEM_PROC);
-	reg_dirty ^= DIAG_CON_MPSS;
-}
-
-void diag_clean_lpass_reg_fn(struct work_struct *work)
-{
-	pr_debug("diag: clean lpass registration\n");
-	reg_dirty |= DIAG_CON_LPASS;
-	diag_clear_reg(LPASS_PROC);
-	reg_dirty ^= DIAG_CON_LPASS;
-}
-
-void diag_clean_wcnss_reg_fn(struct work_struct *work)
-{
-	pr_debug("diag: clean wcnss registration\n");
-	reg_dirty |= DIAG_CON_WCNSS;
-	diag_clear_reg(WCNSS_PROC);
-	reg_dirty ^= DIAG_CON_WCNSS;
-}
-
-void diag_smd_cntl_notify(void *ctxt, unsigned event)
-{
-	int r1, r2;
-
-	if (!(driver->ch_cntl))
+	struct diag_smd_info *smd_info = container_of(work,
+						struct diag_smd_info,
+						diag_notify_update_smd_work);
+	if (!smd_info)
 		return;
 
-	switch (event) {
-	case SMD_EVENT_DATA:
-		r1 = smd_read_avail(driver->ch_cntl);
-		r2 = smd_cur_packet_size(driver->ch_cntl);
-		if (r1 > 0 && r1 == r2)
-			queue_work(driver->diag_wq,
-				 &(driver->diag_read_smd_cntl_work));
-		else
-			pr_debug("diag: incomplete pkt on Modem CNTL ch\n");
-		break;
-	case SMD_EVENT_OPEN:
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_modem_mask_update_work));
-		break;
-	}
+	pr_debug("diag: clean registration for peripheral: %d\n",
+		smd_info->peripheral);
+
+	reg_dirty |= smd_info->peripheral_mask;
+	diag_clear_reg(smd_info->peripheral);
+	reg_dirty ^= smd_info->peripheral_mask;
+
+	smd_info->notify_context = 0;
 }
 
-void diag_smd_lpass_cntl_notify(void *ctxt, unsigned event)
+/* Process the data read from the smd control channel */
+int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
+								int total_recd)
 {
-	int r1, r2;
-
-	if (!(driver->chlpass_cntl))
-		return;
-
-	switch (event) {
-	case SMD_EVENT_DATA:
-		r1 = smd_read_avail(driver->chlpass_cntl);
-		r2 = smd_cur_packet_size(driver->chlpass_cntl);
-		if (r1 > 0 && r1 == r2)
-			queue_work(driver->diag_wq,
-				 &(driver->diag_read_smd_lpass_cntl_work));
-		else
-			pr_debug("diag: incomplete pkt on LPASS CNTL ch\n");
-		break;
-	case SMD_EVENT_OPEN:
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_lpass_mask_update_work));
-		break;
-	}
-}
-
-void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
-{
-	int r1, r2;
-
-	if (!(driver->ch_wcnss_cntl))
-		return;
-
-	switch (event) {
-	case SMD_EVENT_DATA:
-		r1 = smd_read_avail(driver->ch_wcnss_cntl);
-		r2 = smd_cur_packet_size(driver->ch_wcnss_cntl);
-		if (r1 > 0 && r1 == r2)
-			queue_work(driver->diag_wq,
-				 &(driver->diag_read_smd_wcnss_cntl_work));
-		else
-			pr_debug("diag: incomplete pkt on WCNSS CNTL ch\n");
-		break;
-	case SMD_EVENT_OPEN:
-		queue_work(driver->diag_cntl_wq,
-			 &(driver->diag_wcnss_mask_update_work));
-		break;
-	}
-}
-
-static void diag_smd_cntl_send_req(int proc_num)
-{
-	int data_len = 0, type = -1, count_bytes = 0, j, r, flag = 0;
+	int data_len = 0, type = -1, count_bytes = 0, j, flag = 0;
 	struct bindpkt_params_per_process *pkt_params =
-		 kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL);
+		kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL);
 	struct diag_ctrl_msg *msg;
 	struct cmd_code_range *range;
 	struct bindpkt_params *temp;
-	void *buf = NULL;
-	smd_channel_t *smd_ch = NULL;
-	/* tracks which peripheral is sending registration */
-	uint16_t reg_mask = 0;
 
 	if (pkt_params == NULL) {
-		pr_alert("diag: Memory allocation failure\n");
-		return;
+		pr_alert("diag: In %s, Memory allocation failure\n",
+			__func__);
+		return 0;
 	}
 
-	if (proc_num == MODEM_PROC) {
-		buf = driver->buf_in_cntl;
-		smd_ch = driver->ch_cntl;
-		reg_mask = DIAG_CON_MPSS;
-	} else if (proc_num == LPASS_PROC) {
-		buf = driver->buf_in_lpass_cntl;
-		smd_ch = driver->chlpass_cntl;
-		reg_mask = DIAG_CON_LPASS;
-	} else if (proc_num == WCNSS_PROC) {
-		buf = driver->buf_in_wcnss_cntl;
-		smd_ch = driver->ch_wcnss_cntl;
-		reg_mask = DIAG_CON_WCNSS;
-	}
-
-	if (!smd_ch || !buf) {
+	if (!smd_info) {
+		pr_err("diag: In %s, No smd info. Not able to read.\n",
+			__func__);
 		kfree(pkt_params);
-		return;
+		return 0;
 	}
 
-	r = smd_read_avail(smd_ch);
-	if (r > IN_BUF_SIZE) {
-		if (r < MAX_IN_BUF_SIZE) {
-			pr_err("diag: SMD CNTL sending pkt upto %d bytes", r);
-			buf = krealloc(buf, r, GFP_KERNEL);
-		} else {
-			pr_err("diag: CNTL pkt > %d bytes", MAX_IN_BUF_SIZE);
-			kfree(pkt_params);
-			return;
+	while (count_bytes + HDR_SIZ <= total_recd) {
+		type = *(uint32_t *)(buf);
+		data_len = *(uint32_t *)(buf + 4);
+		if (type < DIAG_CTRL_MSG_REG ||
+				 type > DIAG_CTRL_MSG_F3_MASK_V2) {
+			pr_alert("diag: In %s, Invalid Msg type %d proc %d",
+				 __func__, type, smd_info->peripheral);
+			break;
 		}
-	}
-	if (buf && r > 0) {
-		smd_read(smd_ch, buf, r);
-		while (count_bytes + HDR_SIZ <= r) {
-			type = *(uint32_t *)(buf);
-			data_len = *(uint32_t *)(buf + 4);
-			if (type < DIAG_CTRL_MSG_REG ||
-					 type > DIAG_CTRL_MSG_F3_MASK_V2) {
-				pr_alert("diag: Invalid Msg type %d proc %d",
-					 type, proc_num);
-				break;
-			}
-			if (data_len < 0 || data_len > r) {
-				pr_alert("diag: Invalid data len %d proc %d",
-					 data_len, proc_num);
-				break;
-			}
-			count_bytes = count_bytes+HDR_SIZ+data_len;
-			if (type == DIAG_CTRL_MSG_REG && r >= count_bytes) {
-				msg = buf+HDR_SIZ;
-				range = buf+HDR_SIZ+
-						sizeof(struct diag_ctrl_msg);
-				pkt_params->count = msg->count_entries;
-				temp = kzalloc(pkt_params->count * sizeof(struct
-						 bindpkt_params), GFP_KERNEL);
-				if (temp == NULL) {
-					pr_alert("diag: Memory alloc fail\n");
-					kfree(pkt_params);
-					return;
-				}
-				for (j = 0; j < pkt_params->count; j++) {
-					temp->cmd_code = msg->cmd_code;
-					temp->subsys_id = msg->subsysid;
-					temp->client_id = proc_num;
-					temp->proc_id = proc_num;
-					temp->cmd_code_lo = range->cmd_code_lo;
-					temp->cmd_code_hi = range->cmd_code_hi;
-					range++;
-					temp++;
-				}
-				temp -= pkt_params->count;
-				pkt_params->params = temp;
-				flag = 1;
-				/* peripheral undergoing SSR should not
-				 * record new registration
-				 */
-				if (!(reg_dirty & reg_mask))
-					diagchar_ioctl(NULL,
-					 DIAG_IOCTL_COMMAND_REG, (unsigned long)
-								pkt_params);
-				else
-					pr_err("diag: drop reg proc %d\n",
-								 proc_num);
-				kfree(temp);
-			} else if (type != DIAG_CTRL_MSG_REG) {
-				flag = 1;
-			}
-			buf = buf + HDR_SIZ + data_len;
+		if (data_len < 0 || data_len > total_recd) {
+			pr_alert("diag: In %s, Invalid data len %d, total_recd: %d, proc %d",
+				 __func__, data_len, total_recd,
+				 smd_info->peripheral);
+			break;
 		}
+		count_bytes = count_bytes+HDR_SIZ+data_len;
+		if (type == DIAG_CTRL_MSG_REG && total_recd >= count_bytes) {
+			msg = buf+HDR_SIZ;
+			range = buf+HDR_SIZ+
+					sizeof(struct diag_ctrl_msg);
+			pkt_params->count = msg->count_entries;
+			temp = kzalloc(pkt_params->count * sizeof(struct
+					 bindpkt_params), GFP_KERNEL);
+			if (temp == NULL) {
+				pr_alert("diag: In %s, Memory alloc fail\n",
+					__func__);
+				kfree(pkt_params);
+				return flag;
+			}
+			for (j = 0; j < pkt_params->count; j++) {
+				temp->cmd_code = msg->cmd_code;
+				temp->subsys_id = msg->subsysid;
+				temp->client_id = smd_info->peripheral;
+				temp->proc_id = NON_APPS_PROC;
+				temp->cmd_code_lo = range->cmd_code_lo;
+				temp->cmd_code_hi = range->cmd_code_hi;
+				range++;
+				temp++;
+			}
+			temp -= pkt_params->count;
+			pkt_params->params = temp;
+			flag = 1;
+			/* peripheral undergoing SSR should not
+			 * record new registration
+			 */
+			if (!(reg_dirty & smd_info->peripheral_mask))
+				diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
+						(unsigned long)pkt_params);
+			else
+				pr_err("diag: drop reg proc %d\n",
+						smd_info->peripheral);
+			kfree(temp);
+		} else if (type != DIAG_CTRL_MSG_REG) {
+			flag = 1;
+		}
+		buf = buf + HDR_SIZ + data_len;
 	}
 	kfree(pkt_params);
-	if (flag) {
-		/* Poll SMD CNTL channels to check for data */
-		if (proc_num == MODEM_PROC)
-			diag_smd_cntl_notify(NULL, SMD_EVENT_DATA);
-		else if (proc_num == LPASS_PROC)
-			diag_smd_lpass_cntl_notify(NULL, SMD_EVENT_DATA);
-		else if (proc_num == WCNSS_PROC)
-			diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA);
-	}
-}
 
-void diag_read_smd_cntl_work_fn(struct work_struct *work)
-{
-	diag_smd_cntl_send_req(MODEM_PROC);
-}
-
-void diag_read_smd_lpass_cntl_work_fn(struct work_struct *work)
-{
-	diag_smd_cntl_send_req(LPASS_PROC);
-}
-
-void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *work)
-{
-	diag_smd_cntl_send_req(WCNSS_PROC);
+	return flag;
 }
 
 static int diag_smd_cntl_probe(struct platform_device *pdev)
 {
 	int r = 0;
+	int index = -1;
 
 	/* open control ports only on 8960 & newer targets */
 	if (chk_apps_only()) {
-		if (pdev->id == SMD_APPS_MODEM)
-			r = smd_open("DIAG_CNTL", &driver->ch_cntl, driver,
-							diag_smd_cntl_notify);
-		if (pdev->id == SMD_APPS_QDSP)
-			r = smd_named_open_on_edge("DIAG_CNTL", SMD_APPS_QDSP
-					, &driver->chlpass_cntl, driver,
-					diag_smd_lpass_cntl_notify);
-		if (pdev->id == SMD_APPS_WCNSS)
+		if (pdev->id == SMD_APPS_MODEM) {
+			index = MODEM_DATA;
+			r = smd_open("DIAG_CNTL",
+					&driver->smd_cntl[index].ch,
+					&driver->smd_cntl[index],
+					diag_smd_notify);
+			driver->smd_cntl[index].ch_save =
+					driver->smd_cntl[index].ch;
+		} else if (pdev->id == SMD_APPS_QDSP) {
+			index = LPASS_DATA;
+			r = smd_named_open_on_edge("DIAG_CNTL",
+					SMD_APPS_QDSP,
+					&driver->smd_cntl[index].ch,
+					&driver->smd_cntl[index],
+					diag_smd_notify);
+			driver->smd_cntl[index].ch_save =
+					driver->smd_cntl[index].ch;
+		} else if (pdev->id == SMD_APPS_WCNSS) {
+			index = WCNSS_DATA;
 			r = smd_named_open_on_edge("APPS_RIVA_CTRL",
-				SMD_APPS_WCNSS, &driver->ch_wcnss_cntl,
-					driver, diag_smd_wcnss_cntl_notify);
+					SMD_APPS_WCNSS,
+					&driver->smd_cntl[index].ch,
+					&driver->smd_cntl[index],
+					diag_smd_notify);
+			driver->smd_cntl[index].ch_save =
+					driver->smd_cntl[index].ch;
+		}
+
 		pr_debug("diag: open CNTL port, ID = %d,r = %d\n", pdev->id, r);
 	}
 	return 0;
@@ -312,53 +204,51 @@
 
 void diagfwd_cntl_init(void)
 {
+	int success;
+	int i;
+
 	reg_dirty = 0;
 	driver->polling_reg_flag = 0;
 	driver->diag_cntl_wq = create_singlethread_workqueue("diag_cntl_wq");
-	if (driver->buf_in_cntl == NULL) {
-		driver->buf_in_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_cntl == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_cntl);
-	}
-	if (driver->buf_in_lpass_cntl == NULL) {
-		driver->buf_in_lpass_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_lpass_cntl == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_lpass_cntl);
-	}
-	if (driver->buf_in_wcnss_cntl == NULL) {
-		driver->buf_in_wcnss_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
-		if (driver->buf_in_wcnss_cntl == NULL)
-			goto err;
-		kmemleak_not_leak(driver->buf_in_wcnss_cntl);
-	}
+
+	success = diag_smd_constructor(&driver->smd_cntl[MODEM_DATA],
+					MODEM_DATA, SMD_CNTL_TYPE);
+	if (!success)
+		goto err;
+
+	success = diag_smd_constructor(&driver->smd_cntl[LPASS_DATA],
+					LPASS_DATA, SMD_CNTL_TYPE);
+	if (!success)
+		goto err;
+
+	success = diag_smd_constructor(&driver->smd_cntl[WCNSS_DATA],
+					WCNSS_DATA, SMD_CNTL_TYPE);
+	if (!success)
+		goto err;
+
 	platform_driver_register(&msm_smd_ch1_cntl_driver);
 	platform_driver_register(&diag_smd_lite_cntl_driver);
 
 	return;
 err:
-		pr_err("diag: Could not initialize diag buffers");
-		kfree(driver->buf_in_cntl);
-		kfree(driver->buf_in_lpass_cntl);
-		kfree(driver->buf_in_wcnss_cntl);
-		if (driver->diag_cntl_wq)
-			destroy_workqueue(driver->diag_cntl_wq);
+	pr_err("diag: Could not initialize diag buffers");
+
+	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_cntl[i]);
+
+	if (driver->diag_cntl_wq)
+		destroy_workqueue(driver->diag_cntl_wq);
 }
 
 void diagfwd_cntl_exit(void)
 {
-	smd_close(driver->ch_cntl);
-	smd_close(driver->chlpass_cntl);
-	smd_close(driver->ch_wcnss_cntl);
-	driver->ch_cntl = 0;
-	driver->chlpass_cntl = 0;
-	driver->ch_wcnss_cntl = 0;
+	int i;
+
+	for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
+		diag_smd_destructor(&driver->smd_cntl[i]);
+
 	destroy_workqueue(driver->diag_cntl_wq);
+
 	platform_driver_unregister(&msm_smd_ch1_cntl_driver);
 	platform_driver_unregister(&diag_smd_lite_cntl_driver);
-
-	kfree(driver->buf_in_cntl);
-	kfree(driver->buf_in_lpass_cntl);
-	kfree(driver->buf_in_wcnss_cntl);
 }
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index 8a0ec3f..aeb4ba1 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -80,13 +80,8 @@
 void diagfwd_cntl_init(void);
 void diagfwd_cntl_exit(void);
 void diag_read_smd_cntl_work_fn(struct work_struct *);
-void diag_read_smd_lpass_cntl_work_fn(struct work_struct *);
-void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *);
-void diag_smd_cntl_notify(void *ctxt, unsigned event);
-void diag_smd_lpass_cntl_notify(void *ctxt, unsigned event);
-void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event);
-void diag_clean_modem_reg_fn(struct work_struct *);
-void diag_clean_lpass_reg_fn(struct work_struct *);
-void diag_clean_wcnss_reg_fn(struct work_struct *);
+void diag_clean_reg_fn(struct work_struct *work);
+int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
+								int total_recd);
 
 #endif
diff --git a/drivers/cpufreq/cpufreq_ondemand.c b/drivers/cpufreq/cpufreq_ondemand.c
index 8f3c107..8a75cd9 100644
--- a/drivers/cpufreq/cpufreq_ondemand.c
+++ b/drivers/cpufreq/cpufreq_ondemand.c
@@ -1160,7 +1160,6 @@
 		dbs_timer_exit(this_dbs_info);
 
 		mutex_lock(&dbs_mutex);
-		mutex_destroy(&this_dbs_info->timer_mutex);
 		dbs_enable--;
 		/* If device is being removed, policy is no longer
 		 * valid. */
@@ -1239,7 +1238,14 @@
 
 static void __exit cpufreq_gov_dbs_exit(void)
 {
+	unsigned int i;
+
 	cpufreq_unregister_governor(&cpufreq_gov_ondemand);
+	for_each_possible_cpu(i) {
+		struct cpu_dbs_info_s *this_dbs_info =
+			&per_cpu(od_cpu_dbs_info, i);
+		mutex_destroy(&this_dbs_info->timer_mutex);
+	}
 	destroy_workqueue(input_wq);
 }
 
diff --git a/drivers/gpio/gpio-msm-common.c b/drivers/gpio/gpio-msm-common.c
index 800c376..150c434 100644
--- a/drivers/gpio/gpio-msm-common.c
+++ b/drivers/gpio/gpio-msm-common.c
@@ -48,6 +48,8 @@
 };
 #endif
 
+static int tlmm_msm_summary_irq;
+
 struct tlmm_field_cfg {
 	enum msm_tlmm_register reg;
 	u8                     off;
@@ -381,12 +383,12 @@
 
 	if (on) {
 		if (bitmap_empty(msm_gpio.wake_irqs, NR_MSM_GPIOS))
-			irq_set_irq_wake(TLMM_MSM_SUMMARY_IRQ, 1);
+			irq_set_irq_wake(tlmm_msm_summary_irq, 1);
 		set_bit(gpio, msm_gpio.wake_irqs);
 	} else {
 		clear_bit(gpio, msm_gpio.wake_irqs);
 		if (bitmap_empty(msm_gpio.wake_irqs, NR_MSM_GPIOS))
-			irq_set_irq_wake(TLMM_MSM_SUMMARY_IRQ, 0);
+			irq_set_irq_wake(tlmm_msm_summary_irq, 0);
 	}
 
 	if (msm_gpio_irq_extn.irq_set_wake)
@@ -540,6 +542,11 @@
 #ifndef CONFIG_OF
 	int irq, i;
 #endif
+	tlmm_msm_summary_irq = platform_get_irq(pdev, 0);
+	if (tlmm_msm_summary_irq < 0) {
+		pr_err("%s: No interrupt defined for msmgpio\n", __func__);
+		return -ENXIO;
+	}
 	msm_gpio.gpio_chip.dev = &pdev->dev;
 	spin_lock_init(&tlmm_lock);
 	bitmap_zero(msm_gpio.enabled_irqs, NR_MSM_GPIOS);
@@ -558,10 +565,10 @@
 		set_irq_flags(irq, IRQF_VALID);
 	}
 #endif
-	ret = request_irq(TLMM_MSM_SUMMARY_IRQ, msm_summary_irq_handler,
+	ret = request_irq(tlmm_msm_summary_irq, msm_summary_irq_handler,
 			IRQF_TRIGGER_HIGH, "msmgpio", NULL);
 	if (ret) {
-		pr_err("Request_irq failed for TLMM_MSM_SUMMARY_IRQ - %d\n",
+		pr_err("Request_irq failed for tlmm_msm_summary_irq - %d\n",
 				ret);
 		return ret;
 	}
@@ -584,7 +591,7 @@
 	ret = gpiochip_remove(&msm_gpio.gpio_chip);
 	if (ret < 0)
 		return ret;
-	irq_set_handler(TLMM_MSM_SUMMARY_IRQ, NULL);
+	irq_set_handler(tlmm_msm_summary_irq, NULL);
 
 	return 0;
 }
diff --git a/drivers/gpu/ion/ion_cp_heap.c b/drivers/gpu/ion/ion_cp_heap.c
index 96a3cdc..d96b755 100644
--- a/drivers/gpu/ion/ion_cp_heap.c
+++ b/drivers/gpu/ion/ion_cp_heap.c
@@ -104,6 +104,7 @@
 	size_t heap_size;
 	dma_addr_t handle;
 	int cma;
+	int disallow_non_secure_allocation;
 };
 
 enum {
@@ -480,6 +481,13 @@
 		return ION_CP_ALLOCATE_FAIL;
 	}
 
+	if (!secure_allocation && cp_heap->disallow_non_secure_allocation) {
+		mutex_unlock(&cp_heap->lock);
+		pr_debug("%s: non-secure allocation disallowed from this heap\n",
+			__func__);
+		return ION_CP_ALLOCATE_FAIL;
+	}
+
 	/*
 	 * The check above already checked for non-secure allocations when the
 	 * heap is protected. HEAP_PROTECTED implies that this must be a secure
@@ -1286,6 +1294,8 @@
 		cp_heap->iommu_2x_map_domain =
 				extra_data->iommu_2x_map_domain;
 		cp_heap->cma = extra_data->is_cma;
+		cp_heap->disallow_non_secure_allocation =
+			extra_data->no_nonsecure_alloc;
 
 	}
 
diff --git a/drivers/gpu/ion/ion_iommu_heap.c b/drivers/gpu/ion/ion_iommu_heap.c
index 0b691f3..5483054 100644
--- a/drivers/gpu/ion/ion_iommu_heap.c
+++ b/drivers/gpu/ion/ion_iommu_heap.c
@@ -76,7 +76,8 @@
 			goto err2;
 
 		for_each_sg(table->sgl, sg, table->nents, i) {
-			data->pages[i] = alloc_page(GFP_KERNEL | __GFP_ZERO);
+			data->pages[i] = alloc_page(
+				GFP_KERNEL | __GFP_HIGHMEM | __GFP_ZERO);
 			if (!data->pages[i])
 				goto err3;
 
diff --git a/drivers/gpu/ion/msm/msm_ion.c b/drivers/gpu/ion/msm/msm_ion.c
index e422fd26..697587b 100644
--- a/drivers/gpu/ion/msm/msm_ion.c
+++ b/drivers/gpu/ion/msm/msm_ion.c
@@ -777,19 +777,6 @@
 	if (pdata_needs_to_be_freed)
 		free_pdata(pdata);
 
-	/* Check if each heap has been removed from the memblock */
-	for (i = 0; i < num_heaps; i++) {
-		struct ion_platform_heap *heap_data = &pdata->heaps[i];
-		if (!heap_data->base)
-			continue;
-		err = memblock_overlaps_memory(heap_data->base,
-						heap_data->size);
-		if (err) {
-			panic("ION heap %s not removed from memblock\n",
-							heap_data->name);
-		}
-	}
-
 	check_for_heap_overlap(pdata->heaps, num_heaps);
 	platform_set_drvdata(pdev, idev);
 	return 0;
diff --git a/drivers/gpu/msm/adreno.c b/drivers/gpu/msm/adreno.c
index cced7ef..24be1b0 100644
--- a/drivers/gpu/msm/adreno.c
+++ b/drivers/gpu/msm/adreno.c
@@ -141,6 +141,7 @@
  */
 
 #define ANY_ID (~0)
+#define NO_VER (~0)
 
 static const struct {
 	enum adreno_gpurev gpurev;
@@ -150,45 +151,53 @@
 	struct adreno_gpudev *gpudev;
 	unsigned int istore_size;
 	unsigned int pix_shader_start;
-	unsigned int instruction_size; /* Size of an instruction in dwords */
-	unsigned int gmem_size; /* size of gmem for gpu*/
+	/* Size of an instruction in dwords */
+	unsigned int instruction_size;
+	/* size of gmem for gpu*/
+	unsigned int gmem_size;
+	/* version of pm4 microcode that supports sync_lock
+	   between CPU and GPU for SMMU-v1 programming */
+	unsigned int sync_lock_pm4_ver;
+	/* version of pfp microcode that supports sync_lock
+	   between CPU and GPU for SMMU-v1 programming */
+	unsigned int sync_lock_pfp_ver;
 } adreno_gpulist[] = {
 	{ ADRENO_REV_A200, 0, 2, ANY_ID, ANY_ID,
 		"yamato_pm4.fw", "yamato_pfp.fw", &adreno_a2xx_gpudev,
-		512, 384, 3, SZ_256K },
+		512, 384, 3, SZ_256K, NO_VER, NO_VER },
 	{ ADRENO_REV_A203, 0, 1, 1, ANY_ID,
 		"yamato_pm4.fw", "yamato_pfp.fw", &adreno_a2xx_gpudev,
-		512, 384, 3, SZ_256K },
+		512, 384, 3, SZ_256K, NO_VER, NO_VER },
 	{ ADRENO_REV_A205, 0, 1, 0, ANY_ID,
 		"yamato_pm4.fw", "yamato_pfp.fw", &adreno_a2xx_gpudev,
-		512, 384, 3, SZ_256K },
+		512, 384, 3, SZ_256K, NO_VER, NO_VER },
 	{ ADRENO_REV_A220, 2, 1, ANY_ID, ANY_ID,
 		"leia_pm4_470.fw", "leia_pfp_470.fw", &adreno_a2xx_gpudev,
-		512, 384, 3, SZ_512K },
+		512, 384, 3, SZ_512K, NO_VER, NO_VER },
 	/*
 	 * patchlevel 5 (8960v2) needs special pm4 firmware to work around
 	 * a hardware problem.
 	 */
 	{ ADRENO_REV_A225, 2, 2, 0, 5,
 		"a225p5_pm4.fw", "a225_pfp.fw", &adreno_a2xx_gpudev,
-		1536, 768, 3, SZ_512K },
+		1536, 768, 3, SZ_512K, NO_VER, NO_VER },
 	{ ADRENO_REV_A225, 2, 2, 0, 6,
 		"a225_pm4.fw", "a225_pfp.fw", &adreno_a2xx_gpudev,
-		1536, 768, 3, SZ_512K },
+		1536, 768, 3, SZ_512K, 0x225011, 0x225002 },
 	{ ADRENO_REV_A225, 2, 2, ANY_ID, ANY_ID,
 		"a225_pm4.fw", "a225_pfp.fw", &adreno_a2xx_gpudev,
-		1536, 768, 3, SZ_512K },
+		1536, 768, 3, SZ_512K, 0x225011, 0x225002 },
 	/* A3XX doesn't use the pix_shader_start */
 	{ ADRENO_REV_A305, 3, 0, 5, ANY_ID,
 		"a300_pm4.fw", "a300_pfp.fw", &adreno_a3xx_gpudev,
-		512, 0, 2, SZ_256K },
+		512, 0, 2, SZ_256K, 0x3FF037, 0x3FF016 },
 	/* A3XX doesn't use the pix_shader_start */
 	{ ADRENO_REV_A320, 3, 2, ANY_ID, ANY_ID,
 		"a300_pm4.fw", "a300_pfp.fw", &adreno_a3xx_gpudev,
-		512, 0, 2, SZ_512K },
+		512, 0, 2, SZ_512K, 0x3FF037, 0x3FF016 },
 	{ ADRENO_REV_A330, 3, 3, 0, 0,
 		"a330_pm4.fw", "a330_pfp.fw", &adreno_a3xx_gpudev,
-		512, 0, 2, SZ_1M },
+		512, 0, 2, SZ_1M, NO_VER, NO_VER },
 };
 
 static irqreturn_t adreno_irq_handler(struct kgsl_device *device)
@@ -282,7 +291,7 @@
 					uint32_t flags)
 {
 	unsigned int pt_val, reg_pt_val;
-	unsigned int link[200];
+	unsigned int link[250];
 	unsigned int *cmds = &link[0];
 	int sizedwords = 0;
 	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
@@ -315,6 +324,11 @@
 					device->mmu.setstate_memory.gpuaddr +
 					KGSL_IOMMU_SETSTATE_NOP_OFFSET);
 
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
+	/* Acquire GPU-CPU sync Lock here */
+	cmds += kgsl_mmu_sync_lock(&device->mmu, cmds);
+
 	pt_val = kgsl_mmu_get_pt_base_addr(&device->mmu,
 					device->mmu.hwpagetable);
 	if (flags & KGSL_MMUFLAGS_PTUPDATE) {
@@ -376,6 +390,9 @@
 		}
 	}
 
+	/* Release GPU-CPU sync Lock here */
+	cmds += kgsl_mmu_sync_unlock(&device->mmu, cmds);
+
 	if (cpu_is_msm8960())
 		cmds += adreno_add_change_mh_phys_limit_cmds(cmds,
 			kgsl_mmu_get_reg_gpuaddr(&device->mmu, 0,
@@ -388,6 +405,8 @@
 			device->mmu.setstate_memory.gpuaddr +
 			KGSL_IOMMU_SETSTATE_NOP_OFFSET);
 
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
 	sizedwords += (cmds - &link[0]);
 	if (sizedwords) {
 		/* invalidate all base pointers */
@@ -402,6 +421,11 @@
 		kgsl_mmu_disable_clk_on_ts(&device->mmu,
 		adreno_dev->ringbuffer.timestamp[KGSL_MEMSTORE_GLOBAL], true);
 	}
+
+	if (sizedwords > (ARRAY_SIZE(link))) {
+		KGSL_DRV_ERR(device, "Temp command buffer overflow\n");
+		BUG();
+	}
 }
 
 static void adreno_gpummu_setstate(struct kgsl_device *device,
@@ -574,7 +598,7 @@
 	/* 8x25 returns 0 for minor id, but it should be 1 */
 	if (cpu_is_qsd8x50())
 		patchid = 1;
-	else if (cpu_is_msm8625() && minorid == 0)
+	else if ((cpu_is_msm8625() || cpu_is_msm8625q()) && minorid == 0)
 		minorid = 1;
 
 	chipid |= (minorid << 8) | patchid;
@@ -637,6 +661,7 @@
 	adreno_dev->pix_shader_start = adreno_gpulist[i].pix_shader_start;
 	adreno_dev->instruction_size = adreno_gpulist[i].instruction_size;
 	adreno_dev->gmem_size = adreno_gpulist[i].gmem_size;
+	adreno_dev->gpulist_index = i;
 }
 
 static struct platform_device_id adreno_id_table[] = {
@@ -859,8 +884,8 @@
 	if (adreno_of_read_property(node, "qcom,algo-ss-util-pct",
 		&info->algo_param.ss_util_pct))
 		goto err;
-	if (adreno_of_read_property(node, "qcom,algo-ss-iobusy-conv",
-		&info->algo_param.ss_iobusy_conv))
+	if (adreno_of_read_property(node, "qcom,algo-ss-no-corr-below-freq",
+		&info->algo_param.ss_no_corr_below_freq))
 		goto err;
 
 	if (adreno_of_read_property(node, "qcom,energy-active-coeff-a",
@@ -1186,12 +1211,36 @@
 	/* Identify the specific GPU */
 	adreno_identify_gpu(adreno_dev);
 
+	if (adreno_ringbuffer_read_pm4_ucode(device)) {
+		KGSL_DRV_ERR(device, "Reading pm4 microcode failed %s\n",
+			adreno_dev->pm4_fwfile);
+		BUG_ON(1);
+	}
+
+	if (adreno_ringbuffer_read_pfp_ucode(device)) {
+		KGSL_DRV_ERR(device, "Reading pfp microcode failed %s\n",
+			adreno_dev->pfp_fwfile);
+		BUG_ON(1);
+	}
+
 	if (adreno_dev->gpurev == ADRENO_REV_UNKNOWN) {
 		KGSL_DRV_ERR(device, "Unknown chip ID %x\n",
 			adreno_dev->chip_id);
 		goto error_clk_off;
 	}
 
+
+	/*
+	 * Check if firmware supports the sync lock PM4 packets needed
+	 * for IOMMUv1
+	 */
+
+	if ((adreno_dev->pm4_fw_version >=
+		adreno_gpulist[adreno_dev->gpulist_index].sync_lock_pm4_ver) &&
+		(adreno_dev->pfp_fw_version >=
+		adreno_gpulist[adreno_dev->gpulist_index].sync_lock_pfp_ver))
+		device->mmu.flags |= KGSL_MMU_FLAGS_IOMMU_SYNC;
+
 	/* Set up the MMU */
 	if (adreno_is_a2xx(adreno_dev)) {
 		/*
diff --git a/drivers/gpu/msm/adreno.h b/drivers/gpu/msm/adreno.h
index cf16995..836192c 100644
--- a/drivers/gpu/msm/adreno.h
+++ b/drivers/gpu/msm/adreno.h
@@ -86,9 +86,11 @@
 	const char *pfp_fwfile;
 	unsigned int *pfp_fw;
 	size_t pfp_fw_size;
+	unsigned int pfp_fw_version;
 	const char *pm4_fwfile;
 	unsigned int *pm4_fw;
 	size_t pm4_fw_size;
+	unsigned int pm4_fw_version;
 	struct adreno_ringbuffer ringbuffer;
 	unsigned int mharb;
 	struct adreno_gpudev *gpudev;
@@ -98,6 +100,7 @@
 	unsigned int instruction_size;
 	unsigned int ib_check_level;
 	unsigned int fast_hang_detect;
+	unsigned int gpulist_index;
 	struct ocmem_buf *ocmem_hdl;
 	unsigned int ocmem_base;
 };
@@ -366,4 +369,26 @@
 	return cmds - start;
 }
 
+/*
+ * adreno_idle_cmds - Add pm4 packets for GPU idle
+ * @adreno_dev - Pointer to device structure
+ * @cmds - Pointer to memory where idle commands need to be added
+ */
+static inline int adreno_add_idle_cmds(struct adreno_device *adreno_dev,
+							unsigned int *cmds)
+{
+	unsigned int *start = cmds;
+
+	*cmds++ = cp_type3_packet(CP_WAIT_FOR_IDLE, 1);
+	*cmds++ = 0;
+
+	if ((adreno_dev->gpurev == ADRENO_REV_A305) ||
+		(adreno_dev->gpurev == ADRENO_REV_A320)) {
+		*cmds++ = cp_type3_packet(CP_WAIT_FOR_ME, 1);
+		*cmds++ = 0;
+	}
+
+	return cmds - start;
+}
+
 #endif /*__ADRENO_H */
diff --git a/drivers/gpu/msm/adreno_pm4types.h b/drivers/gpu/msm/adreno_pm4types.h
index 016862b..6ec11ea 100644
--- a/drivers/gpu/msm/adreno_pm4types.h
+++ b/drivers/gpu/msm/adreno_pm4types.h
@@ -142,6 +142,12 @@
 /* copy sequencer instruction memory to system memory */
 #define CP_IM_STORE            0x2c
 
+/* test 2 memory locations to dword values specified */
+#define CP_TEST_TWO_MEMS	0x71
+
+/* PFP waits until the FIFO between the PFP and the ME is empty */
+#define CP_WAIT_FOR_ME		0x13
+
 /*
  * for a20x
  * program an offset that will added to the BIN_BASE value of
diff --git a/drivers/gpu/msm/adreno_ringbuffer.c b/drivers/gpu/msm/adreno_ringbuffer.c
index 8af361a..97b35b0 100644
--- a/drivers/gpu/msm/adreno_ringbuffer.c
+++ b/drivers/gpu/msm/adreno_ringbuffer.c
@@ -209,10 +209,10 @@
 	return (*data != NULL) ? 0 : -ENOMEM;
 }
 
-static int adreno_ringbuffer_load_pm4_ucode(struct kgsl_device *device)
+int adreno_ringbuffer_read_pm4_ucode(struct kgsl_device *device)
 {
 	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
-	int i, ret = 0;
+	int ret = 0;
 
 	if (adreno_dev->pm4_fw == NULL) {
 		int len;
@@ -234,24 +234,41 @@
 
 		adreno_dev->pm4_fw_size = len / sizeof(uint32_t);
 		adreno_dev->pm4_fw = ptr;
+		adreno_dev->pm4_fw_version = adreno_dev->pm4_fw[1];
+	}
+
+err:
+	return ret;
+}
+
+
+int adreno_ringbuffer_load_pm4_ucode(struct kgsl_device *device)
+{
+	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
+	int i;
+
+	if (adreno_dev->pm4_fw == NULL) {
+		int ret = adreno_ringbuffer_read_pm4_ucode(device);
+		if (ret)
+			return ret;
 	}
 
 	KGSL_DRV_INFO(device, "loading pm4 ucode version: %d\n",
-		adreno_dev->pm4_fw[0]);
+		adreno_dev->pm4_fw_version);
 
 	adreno_regwrite(device, REG_CP_DEBUG, CP_DEBUG_DEFAULT);
 	adreno_regwrite(device, REG_CP_ME_RAM_WADDR, 0);
 	for (i = 1; i < adreno_dev->pm4_fw_size; i++)
 		adreno_regwrite(device, REG_CP_ME_RAM_DATA,
-				     adreno_dev->pm4_fw[i]);
-err:
-	return ret;
+			adreno_dev->pm4_fw[i]);
+
+	return 0;
 }
 
-static int adreno_ringbuffer_load_pfp_ucode(struct kgsl_device *device)
+int adreno_ringbuffer_read_pfp_ucode(struct kgsl_device *device)
 {
 	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
-	int i, ret = 0;
+	int ret = 0;
 
 	if (adreno_dev->pfp_fw == NULL) {
 		int len;
@@ -272,18 +289,34 @@
 
 		adreno_dev->pfp_fw_size = len / sizeof(uint32_t);
 		adreno_dev->pfp_fw = ptr;
+		adreno_dev->pfp_fw_version = adreno_dev->pfp_fw[5];
+	}
+
+err:
+	return ret;
+}
+
+int adreno_ringbuffer_load_pfp_ucode(struct kgsl_device *device)
+{
+	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
+	int i;
+
+	if (adreno_dev->pfp_fw == NULL) {
+		int ret = adreno_ringbuffer_read_pfp_ucode(device);
+		if (ret)
+			return ret;
 	}
 
 	KGSL_DRV_INFO(device, "loading pfp ucode version: %d\n",
-		adreno_dev->pfp_fw[0]);
+			adreno_dev->pfp_fw_version);
 
 	adreno_regwrite(device, adreno_dev->gpudev->reg_cp_pfp_ucode_addr, 0);
 	for (i = 1; i < adreno_dev->pfp_fw_size; i++)
 		adreno_regwrite(device,
-			adreno_dev->gpudev->reg_cp_pfp_ucode_data,
-			adreno_dev->pfp_fw[i]);
-err:
-	return ret;
+		adreno_dev->gpudev->reg_cp_pfp_ucode_data,
+		adreno_dev->pfp_fw[i]);
+
+	return 0;
 }
 
 int adreno_ringbuffer_start(struct adreno_ringbuffer *rb, unsigned int init_ram)
@@ -390,7 +423,6 @@
 			     GSL_RB_MEMPTRS_SCRATCH_MASK);
 
 	/* load the CP ucode */
-
 	status = adreno_ringbuffer_load_pm4_ucode(device);
 	if (status != 0)
 		return status;
diff --git a/drivers/gpu/msm/adreno_ringbuffer.h b/drivers/gpu/msm/adreno_ringbuffer.h
index 4f58a15..50d9c25 100644
--- a/drivers/gpu/msm/adreno_ringbuffer.h
+++ b/drivers/gpu/msm/adreno_ringbuffer.h
@@ -130,6 +130,10 @@
 						struct adreno_context *context,
 						unsigned int numcmds);
 
+int adreno_ringbuffer_read_pfp_ucode(struct kgsl_device *device);
+
+int adreno_ringbuffer_read_pm4_ucode(struct kgsl_device *device);
+
 static inline int adreno_ringbuffer_count(struct adreno_ringbuffer *rb,
 	unsigned int rptr)
 {
diff --git a/drivers/gpu/msm/kgsl_iommu.c b/drivers/gpu/msm/kgsl_iommu.c
index 31491d5..818bd63 100644
--- a/drivers/gpu/msm/kgsl_iommu.c
+++ b/drivers/gpu/msm/kgsl_iommu.c
@@ -18,6 +18,9 @@
 #include <linux/iommu.h>
 #include <linux/msm_kgsl.h>
 #include <mach/socinfo.h>
+#include <mach/msm_iomap.h>
+#include <mach/board.h>
+#include <stddef.h>
 
 #include "kgsl.h"
 #include "kgsl_device.h"
@@ -27,6 +30,8 @@
 #include "adreno_pm4types.h"
 #include "adreno.h"
 #include "kgsl_trace.h"
+#include "z180.h"
+
 
 static struct kgsl_iommu_register_list kgsl_iommuv1_reg[KGSL_IOMMU_REG_MAX] = {
 	{ 0, 0, 0 },				/* GLOBAL_BASE */
@@ -46,6 +51,8 @@
 	{ 0x008, 0, 0 }				/* RESUME */
 };
 
+struct remote_iommu_petersons_spinlock kgsl_iommu_sync_lock_vars;
+
 static int get_iommu_unit(struct device *dev, struct kgsl_mmu **mmu_out,
 			struct kgsl_iommu_unit **iommu_unit_out)
 {
@@ -93,8 +100,168 @@
 	return NULL;
 }
 
+/* These functions help find the nearest allocated memory entries on either side
+ * of a faulting address. If we know the nearby allocations memory we can
+ * get a better determination of what we think should have been located in the
+ * faulting region
+ */
+
+/*
+ * A local structure to make it easy to store the interesting bits for the
+ * memory entries on either side of the faulting address
+ */
+
+struct _mem_entry {
+	unsigned int gpuaddr;
+	unsigned int size;
+	unsigned int flags;
+	pid_t pid;
+};
+
+/*
+ * Find the closest alloated memory block with an smaller GPU address then the
+ * given address
+ */
+
+static void _prev_entry(struct kgsl_process_private *priv,
+	unsigned int faultaddr, struct _mem_entry *ret)
+{
+	struct rb_node *node;
+	struct kgsl_mem_entry *entry;
+
+	for (node = rb_first(&priv->mem_rb); node; ) {
+		entry = rb_entry(node, struct kgsl_mem_entry, node);
+
+		if (entry->memdesc.gpuaddr > faultaddr)
+			break;
+
+		/*
+		 * If this is closer to the faulting address, then copy
+		 * the entry
+		 */
+
+		if (entry->memdesc.gpuaddr > ret->gpuaddr) {
+			ret->gpuaddr = entry->memdesc.gpuaddr;
+			ret->size = entry->memdesc.size;
+			ret->flags = entry->flags;
+			ret->pid = priv->pid;
+		}
+
+		node = rb_next(&entry->node);
+	}
+}
+
+/*
+ * Find the closest alloated memory block with a greater starting GPU address
+ * then the given address
+ */
+
+static void _next_entry(struct kgsl_process_private *priv,
+	unsigned int faultaddr, struct _mem_entry *ret)
+{
+	struct rb_node *node;
+	struct kgsl_mem_entry *entry;
+
+	for (node = rb_last(&priv->mem_rb); node; ) {
+		entry = rb_entry(node, struct kgsl_mem_entry, node);
+
+		if (entry->memdesc.gpuaddr < faultaddr)
+			break;
+
+		/*
+		 * If this is closer to the faulting address, then copy
+		 * the entry
+		 */
+
+		if (entry->memdesc.gpuaddr < ret->gpuaddr) {
+			ret->gpuaddr = entry->memdesc.gpuaddr;
+			ret->size = entry->memdesc.size;
+			ret->flags = entry->flags;
+			ret->pid = priv->pid;
+		}
+
+		node = rb_prev(&entry->node);
+	}
+}
+
+static void _find_mem_entries(struct kgsl_mmu *mmu, unsigned int faultaddr,
+	unsigned int ptbase, struct _mem_entry *preventry,
+	struct _mem_entry *nextentry)
+{
+	struct kgsl_process_private *private;
+	int id = kgsl_mmu_get_ptname_from_ptbase(mmu, ptbase);
+
+	memset(preventry, 0, sizeof(*preventry));
+	memset(nextentry, 0, sizeof(*nextentry));
+
+	/* Set the maximum possible size as an initial value */
+	nextentry->gpuaddr = 0xFFFFFFFF;
+
+	mutex_lock(&kgsl_driver.process_mutex);
+
+	list_for_each_entry(private, &kgsl_driver.process_list, list) {
+
+		if (private->pagetable->name != id)
+			continue;
+
+		spin_lock(&private->mem_lock);
+		_prev_entry(private, faultaddr, preventry);
+		_next_entry(private, faultaddr, nextentry);
+		spin_unlock(&private->mem_lock);
+	}
+
+	mutex_unlock(&kgsl_driver.process_mutex);
+}
+
+static void _print_entry(struct kgsl_device *device, struct _mem_entry *entry)
+{
+	char name[32];
+	memset(name, 0, sizeof(name));
+
+	kgsl_get_memory_usage(name, sizeof(name) - 1, entry->flags);
+
+	KGSL_LOG_DUMP(device,
+		"[%8.8X - %8.8X] (pid = %d) (%s)\n",
+		entry->gpuaddr,
+		entry->gpuaddr + entry->size,
+		entry->pid, name);
+}
+
+static void _check_if_freed(struct kgsl_iommu_device *iommu_dev,
+	unsigned long addr, unsigned int pid)
+{
+	void *base = kgsl_driver.memfree_hist.base_hist_rb;
+	struct kgsl_memfree_hist_elem *wptr;
+	struct kgsl_memfree_hist_elem *p;
+
+	mutex_lock(&kgsl_driver.memfree_hist_mutex);
+	wptr = kgsl_driver.memfree_hist.wptr;
+	p = wptr;
+	for (;;) {
+		if (p->size && p->pid == pid)
+			if (addr >= p->gpuaddr &&
+				addr < (p->gpuaddr + p->size)) {
+
+				KGSL_LOG_DUMP(iommu_dev->kgsldev,
+					"---- premature free ----\n");
+				KGSL_LOG_DUMP(iommu_dev->kgsldev,
+					"[%8.8X-%8.8X] was already freed by pid %d\n",
+					p->gpuaddr,
+					p->gpuaddr + p->size,
+					p->pid);
+			}
+		p++;
+		if ((void *)p >= base + kgsl_driver.memfree_hist.size)
+			p = (struct kgsl_memfree_hist_elem *) base;
+
+		if (p == kgsl_driver.memfree_hist.wptr)
+			break;
+	}
+	mutex_unlock(&kgsl_driver.memfree_hist_mutex);
+}
+
 static int kgsl_iommu_fault_handler(struct iommu_domain *domain,
-	struct device *dev, unsigned long addr, int flags)
+	struct device *dev, unsigned long addr, int flags, void *token)
 {
 	int ret = 0;
 	struct kgsl_mmu *mmu;
@@ -102,6 +269,9 @@
 	struct kgsl_iommu_unit *iommu_unit;
 	struct kgsl_iommu_device *iommu_dev;
 	unsigned int ptbase, fsr;
+	unsigned int pid;
+
+	struct _mem_entry prev, next;
 
 	ret = get_iommu_unit(dev, &mmu, &iommu_unit);
 	if (ret)
@@ -120,12 +290,30 @@
 	fsr = KGSL_IOMMU_GET_CTX_REG(iommu, iommu_unit,
 		iommu_dev->ctx_id, FSR);
 
+	pid = kgsl_mmu_get_ptname_from_ptbase(mmu, ptbase);
 	KGSL_MEM_CRIT(iommu_dev->kgsldev,
-		"GPU PAGE FAULT: addr = %lX pid = %d\n",
-		addr, kgsl_mmu_get_ptname_from_ptbase(mmu, ptbase));
+		"GPU PAGE FAULT: addr = %lX pid = %d\n", addr, pid);
 	KGSL_MEM_CRIT(iommu_dev->kgsldev, "context = %d FSR = %X\n",
 		iommu_dev->ctx_id, fsr);
 
+	_check_if_freed(iommu_dev, addr, pid);
+
+	KGSL_LOG_DUMP(iommu_dev->kgsldev, "---- nearby memory ----\n");
+
+	_find_mem_entries(mmu, addr, ptbase, &prev, &next);
+
+	if (prev.gpuaddr)
+		_print_entry(iommu_dev->kgsldev, &prev);
+	else
+		KGSL_LOG_DUMP(iommu_dev->kgsldev, "*EMPTY*\n");
+
+	KGSL_LOG_DUMP(iommu_dev->kgsldev, " <- fault @ %8.8lX\n", addr);
+
+	if (next.gpuaddr != 0xFFFFFFFF)
+		_print_entry(iommu_dev->kgsldev, &next);
+	else
+		KGSL_LOG_DUMP(iommu_dev->kgsldev, "*EMPTY*\n");
+
 	mmu->fault = 1;
 	iommu_dev->fault = 1;
 
@@ -387,7 +575,7 @@
 		return NULL;
 	} else {
 		iommu_set_fault_handler(iommu_pt->domain,
-			kgsl_iommu_fault_handler);
+			kgsl_iommu_fault_handler, NULL);
 	}
 
 	return iommu_pt;
@@ -546,6 +734,195 @@
 }
 
 /*
+ * kgsl_iommu_start_sync_lock - Initialize some variables during MMU start up
+ * for GPU CPU synchronization
+ * @mmu - Pointer to mmu device
+ *
+ * Return - 0 on success else error code
+ */
+static int kgsl_iommu_start_sync_lock(struct kgsl_mmu *mmu)
+{
+	struct kgsl_iommu *iommu = mmu->priv;
+	uint32_t lock_gpu_addr = 0;
+
+	if (KGSL_DEVICE_3D0 != mmu->device->id ||
+		!msm_soc_version_supports_iommu_v1() ||
+		!kgsl_mmu_is_perprocess() ||
+		iommu->sync_lock_vars)
+		return 0;
+
+	if (!(mmu->flags & KGSL_MMU_FLAGS_IOMMU_SYNC)) {
+		KGSL_DRV_ERR(mmu->device,
+		"The GPU microcode does not support IOMMUv1 sync opcodes\n");
+		return -ENXIO;
+	}
+	/* Store Lock variables GPU address  */
+	lock_gpu_addr = (iommu->sync_lock_desc.gpuaddr +
+			iommu->sync_lock_offset);
+
+	kgsl_iommu_sync_lock_vars.flag[PROC_APPS] = (lock_gpu_addr +
+		(offsetof(struct remote_iommu_petersons_spinlock,
+			flag[PROC_APPS])));
+	kgsl_iommu_sync_lock_vars.flag[PROC_GPU] = (lock_gpu_addr +
+		(offsetof(struct remote_iommu_petersons_spinlock,
+			flag[PROC_GPU])));
+	kgsl_iommu_sync_lock_vars.turn = (lock_gpu_addr +
+		(offsetof(struct remote_iommu_petersons_spinlock, turn)));
+
+	iommu->sync_lock_vars = &kgsl_iommu_sync_lock_vars;
+
+	return 0;
+}
+
+/*
+ * kgsl_get_sync_lock - Init Sync Lock between GPU and CPU
+ * @mmu - Pointer to mmu device
+ *
+ * Return - 0 on success else error code
+ */
+static int kgsl_iommu_init_sync_lock(struct kgsl_mmu *mmu)
+{
+	struct kgsl_iommu *iommu = mmu->device->mmu.priv;
+	int status = 0;
+	uint32_t lock_phy_addr = 0;
+	uint32_t page_offset = 0;
+
+	if (KGSL_DEVICE_3D0 != mmu->device->id ||
+		!msm_soc_version_supports_iommu_v1() ||
+		!kgsl_mmu_is_perprocess())
+		return status;
+
+	/* Return if already initialized */
+	if (iommu->sync_lock_initialized)
+		return status;
+
+	/* Get the physical address of the Lock variables */
+	lock_phy_addr = (msm_iommu_lock_initialize()
+			- MSM_SHARED_RAM_BASE + msm_shared_ram_phys);
+
+	if (!lock_phy_addr) {
+		KGSL_DRV_ERR(mmu->device,
+				"GPU CPU sync lock is not supported by kernel\n");
+		return -ENXIO;
+	}
+
+	/* Align the physical address to PAGE boundary and store the offset */
+	page_offset = (lock_phy_addr & (PAGE_SIZE - 1));
+	lock_phy_addr = (lock_phy_addr & ~(PAGE_SIZE - 1));
+	iommu->sync_lock_desc.physaddr = (unsigned int)lock_phy_addr;
+	iommu->sync_lock_offset = page_offset;
+
+	iommu->sync_lock_desc.size =
+				PAGE_ALIGN(sizeof(kgsl_iommu_sync_lock_vars));
+	status =  memdesc_sg_phys(&iommu->sync_lock_desc,
+				 iommu->sync_lock_desc.physaddr,
+				 iommu->sync_lock_desc.size);
+
+	if (status)
+		return status;
+
+	/* Flag Sync Lock is Initialized  */
+	iommu->sync_lock_initialized = 1;
+
+	return status;
+}
+
+/*
+ * kgsl_iommu_sync_lock - Acquire Sync Lock between GPU and CPU
+ * @mmu - Pointer to mmu device
+ * @cmds - Pointer to array of commands
+ *
+ * Return - int - number of commands.
+ */
+inline unsigned int kgsl_iommu_sync_lock(struct kgsl_mmu *mmu,
+						unsigned int *cmds)
+{
+	struct kgsl_device *device = mmu->device;
+	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
+	struct kgsl_iommu *iommu = mmu->device->mmu.priv;
+	struct remote_iommu_petersons_spinlock *lock_vars =
+					iommu->sync_lock_vars;
+	unsigned int *start = cmds;
+
+	if (!iommu->sync_lock_initialized)
+		return 0;
+
+	*cmds++ = cp_type3_packet(CP_MEM_WRITE, 2);
+	*cmds++ = lock_vars->flag[PROC_GPU];
+	*cmds++ = 1;
+
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
+	*cmds++ = cp_type3_packet(CP_WAIT_REG_MEM, 5);
+	/* MEM SPACE = memory, FUNCTION = equals */
+	*cmds++ = 0x13;
+	*cmds++ = lock_vars->flag[PROC_GPU];
+	*cmds++ = 0x1;
+	*cmds++ = 0x1;
+	*cmds++ = 0x1;
+
+	*cmds++ = cp_type3_packet(CP_MEM_WRITE, 2);
+	*cmds++ = lock_vars->turn;
+	*cmds++ = 0;
+
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
+	*cmds++ = cp_type3_packet(CP_WAIT_REG_MEM, 5);
+	/* MEM SPACE = memory, FUNCTION = equals */
+	*cmds++ = 0x13;
+	*cmds++ = lock_vars->flag[PROC_GPU];
+	*cmds++ = 0x1;
+	*cmds++ = 0x1;
+	*cmds++ = 0x1;
+
+	*cmds++ = cp_type3_packet(CP_TEST_TWO_MEMS, 3);
+	*cmds++ = lock_vars->flag[PROC_APPS];
+	*cmds++ = lock_vars->turn;
+	*cmds++ = 0;
+
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
+	return cmds - start;
+}
+
+/*
+ * kgsl_iommu_sync_lock - Release Sync Lock between GPU and CPU
+ * @mmu - Pointer to mmu device
+ * @cmds - Pointer to array of commands
+ *
+ * Return - int - number of commands.
+ */
+inline unsigned int kgsl_iommu_sync_unlock(struct kgsl_mmu *mmu,
+					unsigned int *cmds)
+{
+	struct kgsl_device *device = mmu->device;
+	struct adreno_device *adreno_dev = ADRENO_DEVICE(device);
+	struct kgsl_iommu *iommu = mmu->device->mmu.priv;
+	struct remote_iommu_petersons_spinlock *lock_vars =
+						iommu->sync_lock_vars;
+	unsigned int *start = cmds;
+
+	if (!iommu->sync_lock_initialized)
+		return 0;
+
+	*cmds++ = cp_type3_packet(CP_MEM_WRITE, 2);
+	*cmds++ = lock_vars->flag[PROC_GPU];
+	*cmds++ = 0;
+
+	*cmds++ = cp_type3_packet(CP_WAIT_REG_MEM, 5);
+	/* MEM SPACE = memory, FUNCTION = equals */
+	*cmds++ = 0x13;
+	*cmds++ = lock_vars->flag[PROC_GPU];
+	*cmds++ = 0x0;
+	*cmds++ = 0x1;
+	*cmds++ = 0x1;
+
+	cmds += adreno_add_idle_cmds(adreno_dev, cmds);
+
+	return cmds - start;
+}
+
+/*
  * kgsl_get_iommu_ctxt - Get device pointer to IOMMU contexts
  * @mmu - Pointer to mmu device
  *
@@ -727,23 +1104,27 @@
 		return 0;
 
 	for (i = 0; i < iommu->unit_count; i++) {
-		iommu->iommu_units[i].reg_map.priv |= KGSL_MEMDESC_GLOBAL;
-		status = kgsl_mmu_map(pt,
+		status = kgsl_mmu_map_global(pt,
 				&(iommu->iommu_units[i].reg_map),
 				GSL_PT_PAGE_RV | GSL_PT_PAGE_WV);
-		if (status) {
-			iommu->iommu_units[i].reg_map.priv &=
-				~KGSL_MEMDESC_GLOBAL;
+		if (status)
 			goto err;
-		}
 	}
+
+	/* Map Lock variables to GPU pagetable */
+	if (iommu->sync_lock_initialized) {
+		status = kgsl_mmu_map_global(pt, &iommu->sync_lock_desc,
+				GSL_PT_PAGE_RV | GSL_PT_PAGE_WV);
+		if (status)
+			goto err;
+	}
+
 	return 0;
 err:
-	for (i--; i >= 0; i--) {
+	for (i--; i >= 0; i--)
 		kgsl_mmu_unmap(pt,
 				&(iommu->iommu_units[i].reg_map));
-		iommu->iommu_units[i].reg_map.priv &= ~KGSL_MEMDESC_GLOBAL;
-	}
+
 	return status;
 }
 
@@ -763,6 +1144,9 @@
 	int i;
 	for (i = 0; i < iommu->unit_count; i++)
 		kgsl_mmu_unmap(pt, &(iommu->iommu_units[i].reg_map));
+
+	if (iommu->sync_lock_desc.gpuaddr)
+		kgsl_mmu_unmap(pt, &iommu->sync_lock_desc);
 }
 
 
@@ -790,6 +1174,9 @@
 	status = kgsl_set_register_map(mmu);
 	if (status)
 		goto done;
+	status = kgsl_iommu_init_sync_lock(mmu);
+	if (status)
+		goto done;
 
 	iommu->iommu_reg_list = kgsl_iommuv1_reg;
 	iommu->ctx_offset = KGSL_IOMMU_CTX_OFFSET_V1;
@@ -887,6 +1274,10 @@
 		if (status)
 			return -ENOMEM;
 	}
+	status = kgsl_iommu_start_sync_lock(mmu);
+	if (status)
+		return status;
+
 	/* We use the GPU MMU to control access to IOMMU registers on 8960 with
 	 * a225, hence we still keep the MMU active on 8960 */
 	if (cpu_is_msm8960()) {
@@ -1068,7 +1459,12 @@
 		if (reg_map->hostptr)
 			iounmap(reg_map->hostptr);
 		kgsl_sg_free(reg_map->sg, reg_map->sglen);
+		reg_map->priv &= ~KGSL_MEMDESC_GLOBAL;
 	}
+	/* clear IOMMU GPU CPU sync structures */
+	kgsl_sg_free(iommu->sync_lock_desc.sg, iommu->sync_lock_desc.sglen);
+	memset(&iommu->sync_lock_desc, 0, sizeof(iommu->sync_lock_desc));
+	iommu->sync_lock_vars = NULL;
 
 	kfree(iommu);
 
@@ -1125,8 +1521,16 @@
 	pt_base &= (iommu->iommu_reg_list[KGSL_IOMMU_CTX_TTBR0].reg_mask <<
 			iommu->iommu_reg_list[KGSL_IOMMU_CTX_TTBR0].reg_shift);
 
-	if (flags & KGSL_MMUFLAGS_PTUPDATE) {
+	/* For v1 SMMU GPU needs to be idle for tlb invalidate as well */
+	if (msm_soc_version_supports_iommu_v1())
 		kgsl_idle(mmu->device);
+
+	/* Acquire GPU-CPU sync Lock here */
+	msm_iommu_lock();
+
+	if (flags & KGSL_MMUFLAGS_PTUPDATE) {
+		if (!msm_soc_version_supports_iommu_v1())
+			kgsl_idle(mmu->device);
 		for (i = 0; i < iommu->unit_count; i++) {
 			/* get the lsb value which should not change when
 			 * changing ttbr0 */
@@ -1151,6 +1555,10 @@
 			mb();
 		}
 	}
+
+	/* Release GPU-CPU sync Lock here */
+	msm_iommu_unlock();
+
 	/* Disable smmu clock */
 	kgsl_iommu_disable_clk_on_ts(mmu, 0, false);
 }
@@ -1203,6 +1611,8 @@
 	/* These callbacks will be set on some chipsets */
 	.mmu_setup_pt = NULL,
 	.mmu_cleanup_pt = NULL,
+	.mmu_sync_lock = kgsl_iommu_sync_lock,
+	.mmu_sync_unlock = kgsl_iommu_sync_unlock,
 };
 
 struct kgsl_mmu_pt_ops iommu_pt_ops = {
diff --git a/drivers/gpu/msm/kgsl_iommu.h b/drivers/gpu/msm/kgsl_iommu.h
index 661b4f0..f539b07 100644
--- a/drivers/gpu/msm/kgsl_iommu.h
+++ b/drivers/gpu/msm/kgsl_iommu.h
@@ -120,6 +120,13 @@
  * @ctx_offset: The context offset to be added to base address when
  * accessing IOMMU registers
  * @iommu_reg_list: List of IOMMU registers { offset, map, shift } array
+ * @sync_lock_vars: Pointer to the IOMMU spinlock for serializing access to the
+ * IOMMU registers
+ * @sync_lock_desc: GPU Memory descriptor for the memory containing the
+ * spinlocks
+ * @sync_lock_offset - The page offset within a page at which the sync
+ * variables are located
+ * @sync_lock_initialized: True if the sync_lock feature is enabled
  */
 struct kgsl_iommu {
 	struct kgsl_iommu_unit iommu_units[KGSL_IOMMU_MAX_UNITS];
@@ -129,6 +136,10 @@
 	struct kgsl_device *device;
 	unsigned int ctx_offset;
 	struct kgsl_iommu_register_list *iommu_reg_list;
+	struct remote_iommu_petersons_spinlock *sync_lock_vars;
+	struct kgsl_memdesc sync_lock_desc;
+	unsigned int sync_lock_offset;
+	bool sync_lock_initialized;
 };
 
 /*
diff --git a/drivers/gpu/msm/kgsl_mmu.h b/drivers/gpu/msm/kgsl_mmu.h
index c8d637e..9d96633 100644
--- a/drivers/gpu/msm/kgsl_mmu.h
+++ b/drivers/gpu/msm/kgsl_mmu.h
@@ -152,6 +152,10 @@
 			struct kgsl_pagetable *pt);
 	void (*mmu_cleanup_pt) (struct kgsl_mmu *mmu,
 			struct kgsl_pagetable *pt);
+	unsigned int (*mmu_sync_lock)
+			(struct kgsl_mmu *mmu, unsigned int *cmds);
+	unsigned int (*mmu_sync_unlock)
+			(struct kgsl_mmu *mmu, unsigned int *cmds);
 };
 
 struct kgsl_mmu_pt_ops {
@@ -166,6 +170,8 @@
 	void (*mmu_destroy_pagetable) (void *pt);
 };
 
+#define KGSL_MMU_FLAGS_IOMMU_SYNC BIT(31)
+
 struct kgsl_mmu {
 	unsigned int     refcnt;
 	uint32_t      flags;
@@ -398,4 +404,24 @@
 	return 0;
 }
 
+static inline int kgsl_mmu_sync_lock(struct kgsl_mmu *mmu,
+				unsigned int *cmds)
+{
+	if ((mmu->flags & KGSL_MMU_FLAGS_IOMMU_SYNC) &&
+		mmu->mmu_ops && mmu->mmu_ops->mmu_sync_lock)
+		return mmu->mmu_ops->mmu_sync_lock(mmu, cmds);
+	else
+		return 0;
+}
+
+static inline int kgsl_mmu_sync_unlock(struct kgsl_mmu *mmu,
+				unsigned int *cmds)
+{
+	if ((mmu->flags & KGSL_MMU_FLAGS_IOMMU_SYNC) &&
+		mmu->mmu_ops && mmu->mmu_ops->mmu_sync_unlock)
+		return mmu->mmu_ops->mmu_sync_unlock(mmu, cmds);
+	else
+		return 0;
+}
+
 #endif /* __KGSL_MMU_H */
diff --git a/drivers/gpu/msm/kgsl_pwrctrl.c b/drivers/gpu/msm/kgsl_pwrctrl.c
index 4919c24..d33df60 100644
--- a/drivers/gpu/msm/kgsl_pwrctrl.c
+++ b/drivers/gpu/msm/kgsl_pwrctrl.c
@@ -93,11 +93,8 @@
 
 static inline int _adjust_pwrlevel(struct kgsl_pwrctrl *pwr, int level)
 {
-	unsigned int max_pwrlevel = max_t(int, pwr->thermal_pwrlevel,
-		pwr->max_pwrlevel);
-
-	unsigned int min_pwrlevel = max_t(int, pwr->thermal_pwrlevel,
-		pwr->min_pwrlevel);
+	int max_pwrlevel = max_t(int, pwr->thermal_pwrlevel, pwr->max_pwrlevel);
+	int min_pwrlevel = max_t(int, pwr->thermal_pwrlevel, pwr->min_pwrlevel);
 
 	if (level < max_pwrlevel)
 		return max_pwrlevel;
diff --git a/drivers/gud/Kconfig b/drivers/gud/Kconfig
index 3a241b7..929ba01d 100644
--- a/drivers/gud/Kconfig
+++ b/drivers/gud/Kconfig
@@ -3,7 +3,6 @@
 #
 config MOBICORE_SUPPORT
 	tristate "Linux MobiCore Support"
-	#depends on ARM_TRUSTZONE
 	---help---
 	  Enable Linux Kernel MobiCore Support
 
@@ -12,14 +11,14 @@
     depends on MOBICORE_SUPPORT
     ---help---
       Enable Debug mode in the MobiCore Driver.
-      It enables printing information about mobicore operations
+      It enables printing information about MobiCore operations
 
 config MOBICORE_VERBOSE
     bool "MobiCore Module verbose debug mode"
     depends on MOBICORE_DEBUG
     ---help---
       Enable Verbose Debug mode in the MobiCore Driver.
-      It enables printing extra information about mobicore operations
+      It enables printing extra information about MobiCore operations
       Beware: this is only useful for debuging deep in the driver because
       it prints too much logs
 
@@ -29,4 +28,3 @@
     depends on MOBICORE_SUPPORT
     ---help---
       Enable Linux Kernel MobiCore API
-
diff --git a/drivers/gud/Makefile b/drivers/gud/Makefile
index ea212c5..3a16bb7 100644
--- a/drivers/gud/Makefile
+++ b/drivers/gud/Makefile
@@ -6,7 +6,11 @@
 obj-$(CONFIG_MOBICORE_API) += mckernelapi.o
 obj-$(CONFIG_MOBICORE_SUPPORT) += mcdrvmodule.o
 
-mcdrvmodule-objs := mobicore_driver/logging.o mobicore_driver/main.o
+mcdrvmodule-objs := mobicore_driver/logging.o \
+		mobicore_driver/ops.o \
+		mobicore_driver/mem.o \
+		mobicore_driver/api.o \
+		mobicore_driver/main.o
 
 mckernelapi-objs := mobicore_kernelapi/main.o \
 		mobicore_kernelapi/clientlib.o \
@@ -15,7 +19,7 @@
 		mobicore_kernelapi/connection.o
 
 # Release mode by default
-ccflags-y := -DNDEBUG
+ccflags-y := -DNDEBUG -include $(PWD)/$(GUD_ROOT_FOLDER)/mobicore_driver/build_tag.h
 ccflags-y += -Wno-declaration-after-statement
 
 ccflags-$(CONFIG_MOBICORE_DEBUG) += -DDEBUG
diff --git a/drivers/gud/mobicore_driver/api.c b/drivers/gud/mobicore_driver/api.c
new file mode 100644
index 0000000..2506bc2
--- /dev/null
+++ b/drivers/gud/mobicore_driver/api.c
@@ -0,0 +1,114 @@
+/* MobiCore driver module.(interface to the secure world SWD)
+ * MobiCore Driver Kernel Module.
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include <linux/module.h>
+
+#include "main.h"
+#include "mem.h"
+#include "debug.h"
+
+
+/*
+ * Map a virtual memory buffer structure to Mobicore
+ * @param instance
+ * @param addr		address of the buffer(NB it must be kernel virtual!)
+ * @param len		buffer length
+ * @param handle	pointer to handle
+ * @param phys_wsm_l2_table	pointer to physical L2 table(?)
+ *
+ * @return 0 if no error
+ *
+ */
+int mobicore_map_vmem(struct mc_instance *instance, void *addr,
+	uint32_t len, uint32_t *handle, uint32_t *phys)
+{
+	return mc_register_wsm_l2(instance, (uint32_t)addr, len,
+		handle, phys);
+}
+EXPORT_SYMBOL(mobicore_map_vmem);
+
+/*
+ * Unmap a virtual memory buffer from mobicore
+ * @param instance
+ * @param handle
+ *
+ * @return 0 if no error
+ *
+ */
+int mobicore_unmap_vmem(struct mc_instance *instance, uint32_t handle)
+{
+	return mc_unregister_wsm_l2(instance, handle);
+}
+EXPORT_SYMBOL(mobicore_unmap_vmem);
+
+/*
+ * Free a WSM buffer allocated with mobicore_allocate_wsm
+ * @param instance
+ * @param handle		handle of the buffer
+ *
+ * @return 0 if no error
+ *
+ */
+int mobicore_free_wsm(struct mc_instance *instance, uint32_t handle)
+{
+	return mc_free_buffer(instance, handle);
+}
+EXPORT_SYMBOL(mobicore_free_wsm);
+
+
+/*
+ * Allocate WSM for given instance
+ *
+ * @param instance		instance
+ * @param requested_size		size of the WSM
+ * @param handle		pointer where the handle will be saved
+ * @param virt_kernel_addr	pointer for the kernel virtual address
+ * @param phys_addr		pointer for the physical address
+ *
+ * @return error code or 0 for success
+ */
+int mobicore_allocate_wsm(struct mc_instance *instance,
+	unsigned long requested_size, uint32_t *handle, void **virt_kernel_addr,
+	void **phys_addr)
+{
+	struct mc_buffer *buffer = NULL;
+
+	/* Setup the WSM buffer structure! */
+	if (mc_get_buffer(instance, &buffer, requested_size))
+		return -EFAULT;
+
+	*handle = buffer->handle;
+	*phys_addr = buffer->phys;
+	*virt_kernel_addr = buffer->addr;
+	return 0;
+}
+EXPORT_SYMBOL(mobicore_allocate_wsm);
+
+/*
+ * Initialize a new mobicore API instance object
+ *
+ * @return Instance or NULL if no allocation was possible.
+ */
+struct mc_instance *mobicore_open(void)
+{
+	return mc_alloc_instance();
+}
+EXPORT_SYMBOL(mobicore_open);
+
+/*
+ * Release a mobicore instance object and all objects related to it
+ * @param instance instance
+ * @return 0 if Ok or -E ERROR
+ */
+int mobicore_release(struct mc_instance *instance)
+{
+	return mc_release_instance(instance);
+}
+EXPORT_SYMBOL(mobicore_release);
+
diff --git a/drivers/gud/mobicore_driver/arm.h b/drivers/gud/mobicore_driver/arm.h
new file mode 100644
index 0000000..0cddc0c
--- /dev/null
+++ b/drivers/gud/mobicore_driver/arm.h
@@ -0,0 +1,72 @@
+/*
+ * MobiCore driver module.(interface to the secure world SWD)
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_ARM_H_
+#define _MC_ARM_H_
+
+#include "debug.h"
+
+/*
+ * ARM Trustzone specific masks and modes
+ * Vanilla Linux is unaware of TrustZone extension.
+ * I.e. arch/arm/include/asm/ptrace.h does not define monitor mode.
+ * Also TZ bits in cpuid are not defined, ARM port uses magic numbers,
+ * see arch/arm/kernel/setup.c
+ */
+#define ARM_MONITOR_MODE		(0b10110)
+#define ARM_SECURITY_EXTENSION_MASK	(0x30)
+
+/* check if CPU supports the ARM TrustZone Security Extensions */
+inline bool has_security_extensions(void)
+{
+	u32 fea = 0;
+	asm volatile(
+		"mrc p15, 0, %[fea], cr0, cr1, 0" :
+		[fea]"=r" (fea));
+
+	MCDRV_DBG_VERBOSE(mcd, "CPU Features: 0x%X", fea);
+
+	/*
+	 * If the CPU features ID has 0 for security features then the CPU
+	 * doesn't support TrustZone at all!
+	 */
+	if ((fea & ARM_SECURITY_EXTENSION_MASK) == 0)
+		return false;
+
+	return true;
+}
+
+/* check if running in secure mode */
+inline bool is_secure_mode(void)
+{
+	u32 cpsr = 0;
+	u32 nsacr = 0;
+
+	asm volatile(
+		"mrc	p15, 0, %[nsacr], cr1, cr1, 2\n"
+		"mrs %[cpsr], cpsr\n" :
+		[nsacr]"=r" (nsacr),
+		[cpsr]"=r"(cpsr));
+
+	MCDRV_DBG_VERBOSE(mcd, "CPRS.M = set to 0x%X\n", cpsr & MODE_MASK);
+	MCDRV_DBG_VERBOSE(mcd, "SCR.NS = set to 0x%X\n", nsacr);
+
+	/*
+	 * If the NSACR contains the reset value(=0) then most likely we are
+	 * running in Secure MODE.
+	 * If the cpsr mode is set to monitor mode then we cannot load!
+	 */
+	if (nsacr == 0 || ((cpsr & MODE_MASK) == ARM_MONITOR_MODE))
+		return true;
+
+	return false;
+}
+
+#endif /* _MC_ARM_H_ */
diff --git a/drivers/gud/mobicore_driver/build_tag.h b/drivers/gud/mobicore_driver/build_tag.h
index 43541bb..a6898f1 100644
--- a/drivers/gud/mobicore_driver/build_tag.h
+++ b/drivers/gud/mobicore_driver/build_tag.h
@@ -1,6 +1,5 @@
-/**
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2012-2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2012-2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -26,4 +25,5 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-#define MOBICORE_COMPONENT_BUILD_TAG "*** GC_MSM8960_Release_V010 ###"
+#define MOBICORE_COMPONENT_BUILD_TAG \
+		"*** GC_MSM8960_Release_V013 ###"
diff --git a/drivers/gud/mobicore_driver/debug.h b/drivers/gud/mobicore_driver/debug.h
new file mode 100644
index 0000000..f166605
--- /dev/null
+++ b/drivers/gud/mobicore_driver/debug.h
@@ -0,0 +1,65 @@
+/*
+ * MobiCore driver module.(interface to the secure world SWD)
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_DEBUG_H_
+#define _MC_DEBUG_H_
+/* Found in main.c */
+extern struct device *mcd;
+
+#define MCDRV_DBG_ERROR(dev, txt, ...) \
+	dev_err(dev, "[%d] %s() ### ERROR: " txt, \
+		task_pid_vnr(current), \
+		__func__, \
+		##__VA_ARGS__)
+
+/* dummy function helper macro. */
+#define DUMMY_FUNCTION()	do {} while (0)
+
+#if defined(DEBUG)
+
+/* #define DEBUG_VERBOSE */
+#if defined(DEBUG_VERBOSE)
+#define MCDRV_DBG_VERBOSE	MCDRV_DBG
+#else
+#define MCDRV_DBG_VERBOSE(...)	DUMMY_FUNCTION()
+#endif
+
+#define MCDRV_DBG(dev, txt, ...) \
+	dev_info(dev, "[%d on CPU%d] %s(): " txt, \
+		 task_pid_vnr(current), \
+		 raw_smp_processor_id(), \
+		 __func__, \
+		 ##__VA_ARGS__)
+
+#define MCDRV_DBG_WARN(dev, txt, ...) \
+	dev_warn(dev, "[%d] %s() WARNING: " txt, \
+		 task_pid_vnr(current), \
+		 __func__, \
+		 ##__VA_ARGS__)
+
+#define MCDRV_ASSERT(cond) \
+	do { \
+		if (unlikely(!(cond))) { \
+			panic("Assertion failed: %s:%d\n", \
+			      __FILE__, __LINE__); \
+		} \
+	} while (0)
+
+#else
+
+#define MCDRV_DBG_VERBOSE(...)	DUMMY_FUNCTION()
+#define MCDRV_DBG(...)		DUMMY_FUNCTION()
+#define MCDRV_DBG_WARN(...)	DUMMY_FUNCTION()
+
+#define MCDRV_ASSERT(...)	DUMMY_FUNCTION()
+
+#endif /* [not] defined(DEBUG) */
+
+#endif /* _MC_DEBUG_H_ */
diff --git a/drivers/gud/mobicore_driver/fastcall.h b/drivers/gud/mobicore_driver/fastcall.h
new file mode 100644
index 0000000..9f360c1
--- /dev/null
+++ b/drivers/gud/mobicore_driver/fastcall.h
@@ -0,0 +1,157 @@
+/*
+ * Header file of MobiCore Driver Kernel Module.
+ *
+ * MobiCore Fast Call interface
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_FASTCALL_H_
+#define _MC_FASTCALL_H_
+
+#include "debug.h"
+
+/*
+ * MobiCore SMCs
+ */
+#define MC_SMC_N_YIELD		0x3 /* Yield to switch from NWd to SWd. */
+#define MC_SMC_N_SIQ		0x4  /* SIQ to switch from NWd to SWd. */
+
+/*
+ * MobiCore fast calls. See MCI documentation
+ */
+#define MC_FC_INIT		-1
+#define MC_FC_INFO		-2
+#define MC_FC_POWER		-3
+#define MC_FC_DUMP		-4
+#define MC_FC_NWD_TRACE		-31 /* Mem trace setup fastcall */
+
+
+/*
+ * return code for fast calls
+ */
+#define MC_FC_RET_OK				0
+#define MC_FC_RET_ERR_INVALID			1
+#define MC_FC_RET_ERR_ALREADY_INITIALIZED	5
+
+
+/* structure wrappers for specific fastcalls */
+
+/* generic fast call parameters */
+union fc_generic {
+	struct {
+		uint32_t cmd;
+		uint32_t param[3];
+	} as_in;
+	struct {
+		uint32_t resp;
+		uint32_t ret;
+		uint32_t param[2];
+	} as_out;
+};
+
+/* fast call init */
+union mc_fc_init {
+	union fc_generic as_generic;
+	struct {
+		uint32_t cmd;
+		uint32_t base;
+		uint32_t nq_info;
+		uint32_t mcp_info;
+	} as_in;
+	struct {
+		uint32_t resp;
+		uint32_t ret;
+		uint32_t rfu[2];
+	} as_out;
+};
+
+/* fast call info parameters */
+union mc_fc_info {
+	union fc_generic as_generic;
+	struct {
+		uint32_t cmd;
+		uint32_t ext_info_id;
+		uint32_t rfu[2];
+	} as_in;
+	struct {
+		uint32_t resp;
+		uint32_t ret;
+		uint32_t state;
+		uint32_t ext_info;
+	} as_out;
+};
+
+/*
+ * _smc() - fast call to MobiCore
+ *
+ * @data: pointer to fast call data
+ */
+static inline long _smc(void *data)
+{
+	union fc_generic fc_generic;
+	memcpy(&fc_generic, data, sizeof(union fc_generic));
+	if (data == NULL)
+		return -EPERM;
+#ifdef MC_SMC_FASTCALL
+	{
+		int ret = 0;
+		ret = smc_fastcall(data, sizeof(union fc_generic));
+	}
+#else
+	{
+		/* SVC expect values in r0-r3 */
+		register u32 reg0 __asm__("r0") = fc_generic.as_in.cmd;
+		register u32 reg1 __asm__("r1") = fc_generic.as_in.param[0];
+		register u32 reg2 __asm__("r2") = fc_generic.as_in.param[1];
+		register u32 reg3 __asm__("r3") = fc_generic.as_in.param[2];
+
+		__asm__ volatile (
+#ifdef MC_ARCH_EXTENSION_SEC
+			/* This pseudo op is supported and required from
+			 * binutils 2.21 on */
+			".arch_extension sec\n"
+#endif
+			"smc 0\n"
+			: "+r"(reg0), "+r"(reg1), "+r"(reg2), "+r"(reg3)
+		);
+
+		/* set response */
+		fc_generic.as_out.resp     = reg0;
+		fc_generic.as_out.ret      = reg1;
+		fc_generic.as_out.param[0] = reg2;
+		fc_generic.as_out.param[1] = reg3;
+		memcpy(data, &fc_generic, sizeof(union fc_generic));
+	}
+#endif
+	return 0;
+}
+
+/*
+ * convert fast call return code to linux driver module error code
+ */
+static inline int convert_fc_ret(uint32_t sret)
+{
+	int ret = -EFAULT;
+
+	switch (sret) {
+	case MC_FC_RET_OK:
+		ret = 0;
+		break;
+	case MC_FC_RET_ERR_INVALID:
+		ret = -EINVAL;
+		break;
+	case MC_FC_RET_ERR_ALREADY_INITIALIZED:
+		ret = -EBUSY;
+		break;
+	default:
+		break;
+	}
+	return ret;
+}
+
+#endif /* _MC_FASTCALL_H_ */
diff --git a/drivers/gud/mobicore_driver/logging.c b/drivers/gud/mobicore_driver/logging.c
index eb44c8a..089b91c 100644
--- a/drivers/gud/mobicore_driver/logging.c
+++ b/drivers/gud/mobicore_driver/logging.c
@@ -1,108 +1,104 @@
-/** MobiCore driver module.(interface to the secure world SWD)
- * @addtogroup MCD_MCDIMPL_KMOD_LOGGING MobiCore Driver Logging Subsystem.
- * @ingroup  MCD_MCDIMPL_KMOD
- * @{
- * @file
+/*
  * MobiCore Driver Logging Subsystem.
- * The logging subsytem provides the interface between the Mobicore trace
+ *
+ * The logging subsystem provides the interface between the Mobicore trace
  * buffer and the Linux log
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#include "mc_drv_module.h"
-#include "mc_drv_module_linux_api.h"
-#include "mc_drv_module_fastcalls.h"
+#include <linux/miscdevice.h>
+#include <linux/moduleparam.h>
+#include <linux/kthread.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/device.h>
 
-/* Default len of the log ring buffer 256KB*/
-#define LOG_BUF_SIZE	(64 * PAGE_SIZE)
+#include "main.h"
+#include "debug.h"
+#include "ops.h"
+#include "logging.h"
+
+/* Default length of the log ring buffer 256KB*/
+#define LOG_BUF_SIZE			(64 * PAGE_SIZE)
 
 /* Max Len of a log line for printing */
-#define LOG_LINE_SIZE	256
+#define LOG_LINE_SIZE			256
 
 static uint32_t log_size = LOG_BUF_SIZE;
-module_param(log_size, uint, 0);
-MODULE_PARM_DESC(log_size, " Size of the MobiCore log ringbuffer "
-						"(or 256KB default).");
 
-/*----------------------------------------------------------------------------*/
+module_param(log_size, uint, 0);
+MODULE_PARM_DESC(log_size, "Size of the MobiCore log ringbuffer(256KB def)");
+
 /* Definitions for log version 2 */
-#define LOG_TYPE_MASK				(0x0007)
-#define LOG_TYPE_CHAR				0
-#define LOG_TYPE_INTEGER			1
+#define LOG_TYPE_MASK			(0x0007)
+#define LOG_TYPE_CHAR			0
+#define LOG_TYPE_INTEGER		1
 /* Field length */
-#define LOG_LENGTH_MASK				(0x00F8)
-#define LOG_LENGTH_SHIFT			3
+#define LOG_LENGTH_MASK			(0x00F8)
+#define LOG_LENGTH_SHIFT		3
 /* Extra attributes */
-#define LOG_EOL					(0x0100)
-#define LOG_INTEGER_DECIMAL			(0x0200)
-#define LOG_INTEGER_SIGNED			(0x0400)
+#define LOG_EOL				(0x0100)
+#define LOG_INTEGER_DECIMAL		(0x0200)
+#define LOG_INTEGER_SIGNED		(0x0400)
 
 struct logmsg_struct {
-	/* Type and format of data */
-	uint16_t ctrl;
-	/* Unique value for each event source */
-	uint16_t source;
-	/* Value, if any */
-	uint32_t log_data;
+	uint16_t ctrl;			/* Type and format of data */
+	uint16_t source;		/* Unique value for each event source */
+	uint32_t log_data;		/* Value, if any */
 };
 
-/** MobiCore log previous position */
-static uint32_t log_pos;
-/** MobiCore log buffer structure */
-static struct mc_trace_buf *log_buf;
-/** Log Thread task structure */
-struct task_struct *log_thread;
-/** Log Line buffer */
-static char *log_line;
+static uint32_t log_pos;		/* MobiCore log previous position */
+static struct mc_trace_buf *log_buf;	/* MobiCore log buffer structure */
+struct task_struct *log_thread;		/* Log Thread task structure */
+static char *log_line;			/* Log Line buffer */
+static uint32_t log_line_len;		/* Log Line buffer current length */
 
-static void log_msg(struct logmsg_struct *msg);
-
-/*----------------------------------------------------------------------------*/
 static void log_eol(void)
 {
 	if (!strnlen(log_line, LOG_LINE_SIZE))
 		return;
-	printk(KERN_INFO "%s\n", log_line);
+	dev_info(mcd, "%s\n", log_line);
+	log_line_len = 0;
 	log_line[0] = 0;
 }
-/*----------------------------------------------------------------------------*/
-/**
- * Put a char to the log line if there is enough space if not then also
- * output the line. Assume nobody else is updating the line! */
+
+/*
+ * Collect chars in log_line buffer and output the buffer when it is full.
+ * No locking needed because only "mobicore_log" thread updates this buffer.
+ */
 static void log_char(char ch)
 {
-	uint32_t len;
 	if (ch == '\n' || ch == '\r') {
 		log_eol();
 		return;
 	}
 
-	if (strnlen(log_line, LOG_LINE_SIZE) >= LOG_LINE_SIZE - 1) {
-		printk(KERN_INFO "%s\n", log_line);
+	if (log_line_len >= LOG_LINE_SIZE - 1) {
+		dev_info(mcd, "%s\n", log_line);
+		log_line_len = 0;
 		log_line[0] = 0;
 	}
 
-	len = strnlen(log_line, LOG_LINE_SIZE);
-	log_line[len] = ch;
-	log_line[len + 1] = 0;
+	log_line[log_line_len] = ch;
+	log_line[log_line_len + 1] = 0;
+	log_line_len++;
 }
 
-/*----------------------------------------------------------------------------*/
-/**
- * Put a string to the log line if there is enough space if not then also
- * output the line. Assume nobody else is updating the line! */
+/*
+ * Put a string to the log line.
+ */
 static void log_str(const char *s)
 {
 	int i;
+
 	for (i = 0; i < strnlen(s, LOG_LINE_SIZE); i++)
 		log_char(s[i]);
 }
 
-/*----------------------------------------------------------------------------*/
 static uint32_t process_v1log(void)
 {
 	char *last_char = log_buf->buff + log_buf->write_pos;
@@ -116,44 +112,28 @@
 	return buff - log_buf->buff;
 }
 
-/*----------------------------------------------------------------------------*/
-static uint32_t process_v2log(void)
-{
-	char *last_msg = log_buf->buff + log_buf->write_pos;
-	char *buff = log_buf->buff + log_pos;
-	while (buff != last_msg) {
-		log_msg((struct logmsg_struct *)buff);
-		buff += sizeof(struct logmsg_struct);
-		/* Wrap around */
-		if (buff + sizeof(struct logmsg_struct) >
-			(char *)log_buf + log_size)
-			buff = log_buf->buff;
-	}
-	return buff - log_buf->buff;
-}
+static const uint8_t HEX2ASCII[16] = {
+	'0', '1', '2', '3', '4', '5', '6', '7',
+	'8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
 
-static const uint8_t HEX2ASCII[16] = { '0', '1', '2', '3', '4', '5', '6', '7',
-				'8', '9', 'a', 'b', 'c', 'd', 'e', 'f' };
-
-/*----------------------------------------------------------------------------*/
 static void dbg_raw_nro(uint32_t format, uint32_t value)
 {
 	int digits = 1;
 	uint32_t base = (format & LOG_INTEGER_DECIMAL) ? 10 : 16;
 	int width = (format & LOG_LENGTH_MASK) >> LOG_LENGTH_SHIFT;
-	int negative = FALSE;
+	int negative = 0;
 	uint32_t digit_base = 1;
 
 	if ((format & LOG_INTEGER_SIGNED) != 0 && ((signed int)value) < 0) {
-			negative = TRUE;
-			value = (uint32_t)(-(signed int)value);
-			width--;
+		negative = 1;
+		value = (uint32_t)(-(signed int)value);
+		width--;
 	}
 
 	/* Find length and divider to get largest digit */
 	while (value / digit_base >= base) {
-			digit_base *= base;
-			digits++;
+		digit_base *= base;
+		digits++;
 	}
 
 	if (width > digits) {
@@ -175,11 +155,11 @@
 	}
 }
 
-/*----------------------------------------------------------------------------*/
 static void log_msg(struct logmsg_struct *msg)
 {
 	unsigned char msgtxt[5];
 	int mpos = 0;
+
 	switch (msg->ctrl & LOG_TYPE_MASK) {
 	case LOG_TYPE_CHAR: {
 		uint32_t ch;
@@ -203,16 +183,27 @@
 		log_eol();
 }
 
-/*----------------------------------------------------------------------------*/
+static uint32_t process_v2log(void)
+{
+	char *last_msg = log_buf->buff + log_buf->write_pos;
+	char *buff = log_buf->buff + log_pos;
+	while (buff != last_msg) {
+		log_msg((struct logmsg_struct *)buff);
+		buff += sizeof(struct logmsg_struct);
+		/* Wrap around */
+		if ((buff + sizeof(struct logmsg_struct)) >
+		    ((char *)log_buf + log_size))
+			buff = log_buf->buff;
+	}
+	return buff - log_buf->buff;
+}
+
+/* log_worker() - Worker thread processing the log_buf buffer. */
 static int log_worker(void *p)
 {
 	if (log_buf == NULL)
 		return -EFAULT;
 
-	/* The thread should have never started */
-	if (log_buf == NULL)
-		return -EFAULT;
-
 	while (!kthread_should_stop()) {
 		if (log_buf->write_pos == log_pos)
 			schedule_timeout_interruptible(MAX_SCHEDULE_TIMEOUT);
@@ -225,23 +216,21 @@
 			log_pos = process_v2log();
 			break;
 		default:
-			MCDRV_DBG_ERROR("Unknown Mobicore log data "
-				"version %d logging disabled.",
-				log_buf->version);
+			MCDRV_DBG_ERROR(mcd, "Unknown Mobicore log data");
 			log_pos = log_buf->write_pos;
-			/* Stop the thread as we have no idea what
-			 * happens next */
+			/*
+			 * Stop the thread as we have no idea what
+			 * happens next
+			 */
 			return -EFAULT;
 		}
 	}
-	MCDRV_DBG("Logging thread stopped!");
+	MCDRV_DBG(mcd, "Logging thread stopped!");
 	return 0;
 }
 
-
-/*----------------------------------------------------------------------------*/
-/**
- * Wakeup the log reader thread
+/*
+ * Wake up the log reader thread
  * This should be called from the places where calls into MobiCore have
  * generated some logs(eg, yield, SIQ...)
  */
@@ -253,75 +242,89 @@
 	wake_up_process(log_thread);
 }
 
-/*----------------------------------------------------------------------------*/
-/**
- * Setup mobicore kernel log. It assumes it's running on CORE 0!
+/*
+ * Setup MobiCore kernel log. It assumes it's running on CORE 0!
  * The fastcall will complain is that is not the case!
  */
-long mobicore_log_setup(void *data)
+long mobicore_log_setup(void)
 {
 	unsigned long phys_log_buf;
 	union fc_generic fc_log;
+	struct sched_param param = { .sched_priority = 1 };
 
+	long ret;
 	log_pos = 0;
 	log_buf = NULL;
 	log_thread = NULL;
 	log_line = NULL;
+	log_line_len = 0;
 
 	/* Sanity check for the log size */
 	if (log_size < PAGE_SIZE)
 		return -EFAULT;
 	else
-		log_size =
-			get_nr_of_pages_for_buffer(NULL, log_size) * PAGE_SIZE;
+		log_size = PAGE_ALIGN(log_size);
 
 	log_line = kzalloc(LOG_LINE_SIZE, GFP_KERNEL);
 	if (IS_ERR(log_line)) {
-		MCDRV_DBG_ERROR("failed to allocate log line!");
+		MCDRV_DBG_ERROR(mcd, "failed to allocate log line!");
 		return -ENOMEM;
 	}
 
 	log_thread = kthread_create(log_worker, NULL, "mobicore_log");
 	if (IS_ERR(log_thread)) {
-		MCDRV_DBG_ERROR("mobicore log thread creation failed!");
-		return -EFAULT;
+		MCDRV_DBG_ERROR(mcd, "MobiCore log thread creation failed!");
+		ret = -EFAULT;
+		goto mobicore_log_setup_log_line;
 	}
 
-	log_pos = 0;
+	sched_setscheduler(log_thread, SCHED_IDLE, &param);
+	/*
+	 * We are going to map this buffer into virtual address space in SWd.
+	 * To reduce complexity there, we use a contiguous buffer.
+	 */
 	log_buf = (void *)__get_free_pages(GFP_KERNEL | __GFP_ZERO,
-					size_to_order(log_size));
+					   get_order(log_size));
 	if (!log_buf) {
-		MCDRV_DBG_ERROR("Failed to get page for logger!");
-		return -ENOMEM;
+		MCDRV_DBG_ERROR(mcd, "Failed to get page for logger!");
+		ret = -ENOMEM;
+		goto mobicore_log_setup_kthread;
 	}
 	phys_log_buf = virt_to_phys(log_buf);
 
 	memset(&fc_log, 0, sizeof(fc_log));
-	fc_log.as_in.cmd      = MC_FC_NWD_TRACE;
+	fc_log.as_in.cmd = MC_FC_NWD_TRACE;
 	fc_log.as_in.param[0] = phys_log_buf;
 	fc_log.as_in.param[1] = log_size;
 
-	MCDRV_DBG("fc_log virt=%p phys=%p ", log_buf, (void *)phys_log_buf);
+	MCDRV_DBG(mcd, "fc_log virt=%p phys=%p ",
+		  log_buf, (void *)phys_log_buf);
 	mc_fastcall(&fc_log);
-	MCDRV_DBG("fc_log out ret=0x%08x", fc_log.as_out.ret);
+	MCDRV_DBG(mcd, "fc_log out ret=0x%08x", fc_log.as_out.ret);
+
 	/* If the setup failed we must free the memory allocated */
 	if (fc_log.as_out.ret) {
-		MCDRV_DBG_ERROR("MobiCore shared traces setup failed!");
-		kthread_stop(log_thread);
-		free_pages((unsigned long)log_buf, size_to_order(log_size));
-
+		MCDRV_DBG_ERROR(mcd, "MobiCore shared traces setup failed!");
+		free_pages((unsigned long)log_buf, get_order(log_size));
 		log_buf = NULL;
-		log_thread = NULL;
-		return -EIO;
+		ret = -EIO;
+		goto mobicore_log_setup_kthread;
 	}
 
-	MCDRV_DBG("fc_log Logger version %u\n", log_buf->version);
+	MCDRV_DBG(mcd, "fc_log Logger version %u\n", log_buf->version);
 	return 0;
+
+mobicore_log_setup_kthread:
+	kthread_stop(log_thread);
+	log_thread = NULL;
+mobicore_log_setup_log_line:
+	kfree(log_line);
+	log_line = NULL;
+	return ret;
 }
 
-/*----------------------------------------------------------------------------*/
-/**
- * Free kernel log componenets.
+/*
+ * Free kernel log components.
  * ATTN: We can't free the log buffer because it's also in use by MobiCore and
  * even if the module is unloaded MobiCore is still running.
  */
diff --git a/drivers/gud/mobicore_driver/logging.h b/drivers/gud/mobicore_driver/logging.h
new file mode 100644
index 0000000..ec7587f
--- /dev/null
+++ b/drivers/gud/mobicore_driver/logging.h
@@ -0,0 +1,27 @@
+/*
+ * MobiCore driver module.(interface to the secure world SWD)
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_LOGGING_H_
+#define _MC_LOGGING_H_
+
+/* MobiCore internal trace buffer structure. */
+struct mc_trace_buf {
+	uint32_t version; /* version of trace buffer */
+	uint32_t length; /* length of allocated buffer(includes header) */
+	uint32_t write_pos; /* last write position */
+	char  buff[1]; /* start of the log buffer */
+};
+
+/* MobiCore internal trace log setup. */
+void mobicore_log_read(void);
+long mobicore_log_setup(void);
+void mobicore_log_free(void);
+
+#endif /* _MC_LOGGING_H_ */
diff --git a/drivers/gud/mobicore_driver/main.c b/drivers/gud/mobicore_driver/main.c
index 8a8ea1e..1a745b3 100644
--- a/drivers/gud/mobicore_driver/main.c
+++ b/drivers/gud/mobicore_driver/main.c
@@ -1,1969 +1,626 @@
-/** MobiCore driver module.(interface to the secure world SWD)
- * @addtogroup MCD_MCDIMPL_KMOD_IMPL
- * @{
- * @file
+/*
  * MobiCore Driver Kernel Module.
- * This module is written as a Linux device driver.
+ *
  * This driver represents the command proxy on the lowest layer, from the
  * secure world to the non secure world, and vice versa.
- * This driver is located in the non secure world (Linux).
+
  * This driver offers IOCTL commands, for access to the secure world, and has
  * the interface from the secure world to the normal world.
  * The access to the driver is possible with a file descriptor,
- * which has to be created by the fd = open(/dev/mobicore) command.
+ * which has to be created by the fd = open(/dev/mobicore) command or
+ * fd = open(/dev/mobicore-user)
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-
+#include <linux/miscdevice.h>
+#include <linux/interrupt.h>
+#include <linux/highmem.h>
+#include <linux/slab.h>
+#include <linux/kthread.h>
+#include <linux/device.h>
 #include <linux/module.h>
-#include "mc_drv_module.h"
-#include "mc_drv_module_linux_api.h"
-#include "mc_drv_module_android.h"
-#include "mc_drv_module_fastcalls.h"
-#include "public/mc_kernel_api.h"
+#include <linux/ioctl.h>
+#include <linux/mm.h>
+#include <linux/mman.h>
+#include <linux/completion.h>
 
-/* Initial value for the daemon sempahore signaling */
-#define DAEMON_SEM_VAL 0
+#include "main.h"
+#include "fastcall.h"
 
-/** MobiCore interrupt context data */
-static struct mc_drv_kmod_ctx	mc_drv_kmod_ctx;
+#include "arm.h"
+#include "mem.h"
+#include "ops.h"
+#include "pm.h"
+#include "debug.h"
+#include "logging.h"
 
-/** MobiCore MCI information */
-static uint32_t mci_base;
-/*
-#############################################################################
-##
-## Convenience functions for Linux API functions
-##
-#############################################################################*/
-static int goto_cpu0(void);
-static int goto_all_cpu(void) __attribute__ ((unused));
+/* Define a MobiCore device structure for use with dev_debug() etc */
+struct device_driver mcd_debug_name = {
+	.name = "mcdrvkmod"
+};
 
+struct device mcd_debug_subname = {
+	.init_name = "", /* Set to 'mcd' at mc_init() time */
+	.driver = &mcd_debug_name
+};
 
-/*----------------------------------------------------------------------------*/
-static void init_and_add_to_list(
-	struct list_head *item,
-	struct list_head *list_head
-)
+struct device *mcd = &mcd_debug_subname;
+
+/* MobiCore interrupt context data */
+struct mc_context ctx;
+
+/* Get process context from file pointer */
+static struct mc_instance *get_instance(struct file *file)
 {
-	INIT_LIST_HEAD(item);
-
-	list_add(item, list_head);
+	return (struct mc_instance *)(file->private_data);
 }
 
-/*----------------------------------------------------------------------------*/
-/** check if CPU supports the ARM TrustZone Security Extensions
- *	@return int TRUE or FALSE */
-static int has_security_extensions(
-	void
-)
+/* Get a unique ID */
+unsigned int get_unique_id(void)
 {
-	u32 fea = 0;
-	asm volatile(
-		"mrc p15, 0, %[fea], cr0, cr1, 0" :
-		[fea]"=r" (fea));
+	return (unsigned int)atomic_inc_return(&ctx.unique_counter);
+}
 
-	MCDRV_DBG_VERBOSE("CPU Features: 0x%X", fea);
+/* Clears the reserved bit of each page and frees the pages */
+static inline void free_continguous_pages(void *addr, unsigned int order)
+{
+	int i;
+	struct page *page = virt_to_page(addr);
+	for (i = 0; i < (1<<order); i++) {
+		MCDRV_DBG_VERBOSE(mcd, "free page at 0x%p\n", page);
+		ClearPageReserved(page);
+		page++;
+	}
 
-	/* If the CPU features ID has 0 for security features then the CPU
-	 * doesn't support TrustZone at all!
-	 */
-	if ((fea & ARM_SECURITY_EXTENSION_MASK) == 0)
+	MCDRV_DBG_VERBOSE(mcd, "freeing addr:%p, order:%x\n", addr, order);
+	free_pages((unsigned long)addr, order);
+}
+
+/* Frees the memory associated with a buffer */
+static int free_buffer(struct mc_buffer *buffer)
+{
+	if (buffer->handle == 0)
+		return -EINVAL;
+
+	if (buffer->addr == 0)
+		return -EINVAL;
+
+	if (!atomic_dec_and_test(&buffer->usage)) {
+
+		MCDRV_DBG_VERBOSE(mcd, "Could not free buffer h=%u",
+				  buffer->handle);
+		return 0;
+	}
+
+	MCDRV_DBG(mcd, "handle=%u phys_addr=0x%p, virt_addr=0x%p\n",
+		  buffer->handle, buffer->phys, buffer->addr);
+
+	list_del(&buffer->list);
+
+	free_continguous_pages(buffer->addr, buffer->order);
+	kfree(buffer);
+	return 0;
+}
+
+static uint32_t mc_find_cont_wsm(struct mc_instance *instance, uint32_t handle,
+	uint32_t *phys, uint32_t *len)
+{
+	int ret = 0;
+	struct mc_buffer *buffer;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
+
+	mutex_lock(&instance->lock);
+
+	mutex_lock(&ctx.bufs_lock);
+
+	/* search for the given handle in the buffers list */
+	list_for_each_entry(buffer, &ctx.cont_bufs, list) {
+		if (buffer->handle == handle) {
+			*phys = (uint32_t)buffer->phys;
+			*len = buffer->len;
+			goto found;
+		}
+	}
+
+	/* Couldn't find the buffer */
+	ret = -EINVAL;
+
+found:
+	mutex_unlock(&ctx.bufs_lock);
+	mutex_unlock(&instance->lock);
+
+	return ret;
+}
+
+/*
+ * __free_buffer - Free a WSM buffer allocated with mobicore_allocate_wsm
+ *
+ * @instance
+ * @handle		handle of the buffer
+ *
+ * Returns 0 if no error
+ *
+ */
+static int __free_buffer(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+	struct mc_buffer *buffer;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	mutex_lock(&ctx.bufs_lock);
+	/* search for the given handle in the buffers list */
+	list_for_each_entry(buffer, &ctx.cont_bufs, list) {
+		if (buffer->handle == handle)
+			goto del_buffer;
+	}
+	ret = -EINVAL;
+	goto err;
+
+del_buffer:
+	ret = free_buffer(buffer);
+err:
+	mutex_unlock(&ctx.bufs_lock);
+	return ret;
+}
+
+int mc_free_buffer(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	mutex_lock(&instance->lock);
+
+	ret = __free_buffer(instance, handle);
+	mutex_unlock(&instance->lock);
+	return ret;
+}
+
+
+int mc_get_buffer(struct mc_instance *instance,
+	struct mc_buffer **buffer, unsigned long len)
+{
+	struct mc_buffer *cbuffer = NULL;
+	void *addr = 0;
+	void *phys = 0;
+	unsigned int order;
+	unsigned long allocated_size;
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (len == 0) {
+		MCDRV_DBG_WARN(mcd, "cannot allocate size 0\n");
+		return -ENOMEM;
+	}
+
+	order = get_order(len);
+	if (order > MAX_ORDER) {
+		MCDRV_DBG_WARN(mcd, "Buffer size too large\n");
+		return -ENOMEM;
+	}
+	allocated_size = (1 << order) * PAGE_SIZE;
+
+	if (mutex_lock_interruptible(&instance->lock))
+		return -ERESTARTSYS;
+
+	/* allocate a new buffer. */
+	cbuffer = kzalloc(sizeof(struct mc_buffer), GFP_KERNEL);
+
+	if (cbuffer == NULL) {
+		MCDRV_DBG_WARN(mcd,
+			       "MMAP_WSM request: could not allocate buffer\n");
+		ret = -ENOMEM;
+		goto unlock_instance;
+	}
+	mutex_lock(&ctx.bufs_lock);
+
+	MCDRV_DBG_VERBOSE(mcd, "size %ld -> order %d --> %ld (2^n pages)\n",
+			  len, order, allocated_size);
+
+	addr = (void *)__get_free_pages(GFP_USER | __GFP_ZERO, order);
+
+	if (addr == NULL) {
+		MCDRV_DBG_WARN(mcd, "get_free_pages failed\n");
+		ret = -ENOMEM;
+		goto err;
+	}
+	phys = (void *)virt_to_phys(addr);
+	cbuffer->handle = get_unique_id();
+	cbuffer->phys = phys;
+	cbuffer->addr = addr;
+	cbuffer->order = order;
+	cbuffer->len = len;
+	cbuffer->instance = instance;
+	/* Refcount +1 because the TLC is requesting it */
+	atomic_set(&cbuffer->usage, 1);
+
+	INIT_LIST_HEAD(&cbuffer->list);
+	list_add(&cbuffer->list, &ctx.cont_bufs);
+
+	MCDRV_DBG(mcd,
+		  "allocated phys=0x%p - 0x%p, size=%ld, kvirt=0x%p, h=%d\n",
+		  phys, (void *)((unsigned int)phys+allocated_size),
+		  allocated_size, addr, cbuffer->handle);
+	*buffer = cbuffer;
+	goto unlock;
+
+err:
+	kfree(cbuffer);
+unlock:
+	mutex_unlock(&ctx.bufs_lock);
+unlock_instance:
+	mutex_unlock(&instance->lock);
+	return ret;
+}
+
+/*
+ * __lock_buffer() - Locks a contiguous buffer - +1 refcount.
+ * Assumes the instance lock is already taken!
+ */
+static int __lock_buffer(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+	struct mc_buffer *buffer;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
+
+	mutex_lock(&ctx.bufs_lock);
+	/* search for the given handle in the buffers list */
+	list_for_each_entry(buffer, &ctx.cont_bufs, list) {
+		if (buffer->handle == handle) {
+			atomic_inc(&buffer->usage);
+			goto unlock;
+		}
+	}
+	ret = -EINVAL;
+
+unlock:
+	mutex_unlock(&ctx.bufs_lock);
+	return ret;
+}
+
+void *get_mci_base_phys(unsigned int len)
+{
+	if (ctx.mci_base.phys) {
+		return ctx.mci_base.phys;
+	} else {
+		unsigned int order = get_order(len);
+		ctx.mcp = NULL;
+		ctx.mci_base.order = order;
+		ctx.mci_base.addr =
+			(void *)__get_free_pages(GFP_USER | __GFP_ZERO, order);
+		if (ctx.mci_base.addr == NULL) {
+			MCDRV_DBG_WARN(mcd, "get_free_pages failed\n");
+			memset(&ctx.mci_base, 0, sizeof(ctx.mci_base));
+			return NULL;
+		}
+		ctx.mci_base.phys = (void *)virt_to_phys(ctx.mci_base.addr);
+		return ctx.mci_base.phys;
+	}
+}
+
+/*
+ * Create a l2 table from a virtual memory buffer which can be vmalloc
+ * or user space virtual memory
+ */
+int mc_register_wsm_l2(struct mc_instance *instance,
+	uint32_t buffer, uint32_t len,
+	uint32_t *handle, uint32_t *phys)
+{
+	int ret = 0;
+	struct mc_l2_table *table = NULL;
+	struct task_struct *task = current;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (len == 0) {
+		MCDRV_DBG_ERROR(mcd, "len=0 is not supported!\n");
+		return -EINVAL;
+	}
+
+	table = mc_alloc_l2_table(instance, task, (void *)buffer, len);
+
+	if (IS_ERR(table)) {
+		MCDRV_DBG_ERROR(mcd, "new_used_l2_table() failed\n");
+		return -EINVAL;
+	}
+
+	/* set response */
+	*handle = table->handle;
+	/* WARNING: daemon shouldn't know this either, but live with it */
+	if (is_daemon(instance))
+		*phys = (uint32_t)table->phys;
+	else
+		*phys = 0;
+
+	MCDRV_DBG_VERBOSE(mcd, "handle: %d, phys=%p\n",
+			  *handle, (void *)*phys);
+
+	MCDRV_DBG_VERBOSE(mcd, "exit with %d/0x%08X\n", ret, ret);
+
+	return ret;
+}
+
+int mc_unregister_wsm_l2(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	/* free table (if no further locks exist) */
+	mc_free_l2_table(instance, handle);
+
+	return ret;
+}
+/* Lock the object from handle, it could be a WSM l2 table or a cont buffer! */
+static int mc_lock_handle(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
+
+	mutex_lock(&instance->lock);
+	ret = mc_lock_l2_table(instance, handle);
+
+	/* Handle was not a l2 table but a cont buffer */
+	if (ret == -EINVAL) {
+		/* Call the non locking variant! */
+		ret = __lock_buffer(instance, handle);
+	}
+
+	mutex_unlock(&instance->lock);
+
+	return ret;
+}
+
+static int mc_unlock_handle(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
+
+	mutex_lock(&instance->lock);
+	ret = mc_free_l2_table(instance, handle);
+
+	/* Not a l2 table, then it must be a buffer */
+	if (ret == -EINVAL) {
+		/* Call the non locking variant! */
+		ret = __free_buffer(instance, handle);
+	}
+	mutex_unlock(&instance->lock);
+
+	return ret;
+}
+
+static uint32_t mc_find_wsm_l2(struct mc_instance *instance, uint32_t handle)
+{
+	uint32_t ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
 		return 0;
 
-	return 1;
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return 0;
+	}
+
+	mutex_lock(&instance->lock);
+	ret = mc_find_l2_table(instance, handle);
+	mutex_unlock(&instance->lock);
+
+	return ret;
 }
 
-/*----------------------------------------------------------------------------*/
-/** check if running in secure mode
- *	@return int TRUE or FALSE */
-static int is_secure_mode(
-	void
-)
+static int mc_clean_wsm_l2(struct mc_instance *instance)
 {
-	u32 cpsr = 0, nsacr = 0;
-	asm volatile(
-		"mrc	p15, 0, %[nsacr], cr1, cr1, 2\n"
-		"mrs %[cpsr], cpsr\n" :
-		[nsacr]"=r" (nsacr),
-		[cpsr]"=r"(cpsr));
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
 
-	MCDRV_DBG_VERBOSE("CPRS.M = set to 0x%X\n", cpsr & ARM_CPSR_MASK);
-	MCDRV_DBG_VERBOSE("SCR.NS = set to 0x%X\n", nsacr);
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
 
-	/* If the NSACR contains the reset value(=0) then most likely we are
-	 * running in Secure MODE.
-	 * If the cpsr mode is set to monitor mode then we cannot load!
-	 */
-	if (nsacr == 0 || ((cpsr & ARM_CPSR_MASK) == ARM_MONITOR_MODE))
-		return 1;
+	mc_clean_l2_tables();
 
 	return 0;
 }
 
-/*----------------------------------------------------------------------------*/
-/** check if userland caller is privileged (aka has "root" access rights).
-	@return int TRUE or FALSE */
-static int is_userland_caller_privileged(
-	void
-) {
-	/* For some platforms we cannot run the Daemon as root - for Android
-	 * compliance tests it is not allowed, thus we assume the daemon is ran
-	 * as the system user.
-	 * In Android the system user for daemons has no particular capabilities
-	 * other than a fixed UID: AID_SYSTEM 1000
-	 * The actual number is guaranteed to be the same in all Android systems
-	 * so we will take it for granted: see android_filesystem_config.h in
-	 * the Android source tree for all UIDs and their meaning:
-	 * http://android-dls.com/wiki/index.php?title=Android_UIDs_and_GIDs
-	 */
-#ifdef MC_ANDROID_UID_CHECK
-	return current_euid() <= AID_SYSTEM;
-#else
-	/* capable should cover all possibilities, root or sudo, uid checking
-	 * was not very reliable */
-	return capable(CAP_SYS_ADMIN);
-#endif
-}
-
-
-
-/*----------------------------------------------------------------------------*/
-static void unlock_page_from_used_l2_table(
-	struct page *page
-){
-	/* REV axh: check if we should do this. */
-	SetPageDirty(page);
-
-	/* release page, old api was page_cache_release() */
-	ClearPageReserved(page);
-	put_page(page);
-}
-
-/*----------------------------------------------------------------------------*/
-/* convert L2 PTE to page pointer */
-static struct page *l2_pte_to_page(
-	pte_t pte
-) {
-	void *phys_page_addr	= (void *)((unsigned int)pte & PAGE_MASK);
-	unsigned int pfn	= addr_to_pfn(phys_page_addr);
-	struct page *page	= pfn_to_page(pfn);
-	return page;
-}
-
-/*----------------------------------------------------------------------------*/
-/* convert page pointer to L2 PTE */
-static pte_t page_to_l2_pte(
-	struct page *page
-)
+static int mc_fd_mmap(struct file *file, struct vm_area_struct *vmarea)
 {
-	unsigned int pfn	= page_to_pfn(page);
-	void *phys_addr		= pfn_to_addr(pfn);
-	pte_t pte		= (pte_t)((unsigned int)phys_addr & PAGE_MASK);
-	return pte;
-}
-
-
-/*----------------------------------------------------------------------------*/
-static inline int lock_user_pages(
-	struct task_struct	*task,
-	void			*virt_start_page_addr,
-	int			nr_of_pages,
-	struct page		**pages
-)
-{
-	int		ret = 0;
-	int		locked_pages = 0;
-	unsigned int	i;
-
-	do {
-
-		/* lock user pages, must hold the mmap_sem to do this. */
-		down_read(&(task->mm->mmap_sem));
-		locked_pages = get_user_pages(
-					  task,
-					  task->mm,
-					  (unsigned long)virt_start_page_addr,
-					  nr_of_pages,
-					  1, /* write access */
-					  0, /* they say drivers should always
-						pass 0 here..... */
-					  pages,
-					  NULL); /* we don't need the VMAs */
-		up_read(&(task->mm->mmap_sem));
-
-		/* could as lock all pages? */
-		if (locked_pages != nr_of_pages) {
-			MCDRV_DBG_ERROR(
-				"get_user_pages() failed, "
-				"locked_pages=%d\n",
-				locked_pages);
-			ret = -ENOMEM;
-			/* check if an error has been returned. */
-			if (locked_pages < 0) {
-				ret = locked_pages;
-				locked_pages = 0;
-			}
-			break;
-		}
-
-		/* do cache maintenance on locked pages. */
-		for (i = 0; i < nr_of_pages; i++)
-			flush_dcache_page(pages[i]);
-
-	} while (FALSE);
-
-
-	if (ret != 0) {
-		/* release all locked pages. */
-		MCDRV_ASSERT(locked_pages >= 0);
-		for (i = 0; i < locked_pages; i++)
-			put_page(pages[i]);
-	}
-
-	return ret;
-
-}
-
-/*
-#############################################################################
-##
-## Driver implementation functions
-##
-#############################################################################*/
-/*----------------------------------------------------------------------------*/
-/* check if caller is MobiCore Daemon */
-static unsigned int is_caller_mc_daemon(
-	struct mc_instance *instance
-)
-{
-	return ((instance != NULL)
-		&& (mc_drv_kmod_ctx.daemon_inst == instance));
-}
-
-
-/*----------------------------------------------------------------------------*/
-/* Get process context from file pointer */
-static struct mc_instance *get_instance(
-	struct file *file
-) {
-	MCDRV_ASSERT(file != NULL);
-
-	return (struct mc_instance *)(file->private_data);
-}
-
-
-/*----------------------------------------------------------------------------*/
-/* Get a unique ID */
-static unsigned int get_mc_kmod_unique_id(
-	void
-)
-{
-	return (unsigned int)atomic_inc_return(
-			   &(mc_drv_kmod_ctx.unique_counter));
-}
-
-
-/*----------------------------------------------------------------------------*/
-/* Get kernel pointer to shared L2 table given a per-process reference */
-static struct l2table *get_l2_table_kernel_virt(
-	struct mc_used_l2_table	*used_l2table
-)
-{
-	MCDRV_ASSERT(used_l2table != NULL);
-	MCDRV_ASSERT(used_l2table->set != NULL);
-	MCDRV_ASSERT(used_l2table->set->kernel_virt != NULL);
-	return &(used_l2table->set->kernel_virt->table[used_l2table->idx]);
-}
-
-/*----------------------------------------------------------------------------*/
-/* Get physical address of a shared L2 table given a per-process reference */
-static struct l2table *get_l2_table_phys(
-	struct mc_used_l2_table  *used_l2table
-)
-{
-	MCDRV_ASSERT(used_l2table != NULL);
-	MCDRV_ASSERT(used_l2table->set != NULL);
-	MCDRV_ASSERT(used_l2table->set->phys != NULL);
-	return &(used_l2table->set->phys->table[used_l2table->idx]);
-}
-
-/*----------------------------------------------------------------------------*/
-static unsigned int is_in_use_used_l2_table(
-	struct mc_used_l2_table	*used_l2table
-)
-{
-	return ((used_l2table->flags &
-			  (MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_APP
-			   | MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC)) != 0);
-}
-
-
-
-/*----------------------------------------------------------------------------*/
-static struct mc_used_l2_table *find_used_l2_table_by_handle(
-	unsigned int	handle
-) {
-	struct mc_used_l2_table  *used_l2table;
-	struct mc_used_l2_table  *used_l2table_with_handle = NULL;
-
-	list_for_each_entry(
-		used_l2table,
-		&(mc_drv_kmod_ctx.mc_used_l2_tables),
-		list
-	) {
-		if (handle == used_l2table->handle) {
-			used_l2table_with_handle = used_l2table;
-			break;
-		}
-	}
-
-	return used_l2table_with_handle;
-}
-
-/*
-#############################################################################
-##
-## L2 Table Pool
-##
-#############################################################################*/
-
-/*----------------------------------------------------------------------------*/
-static struct mc_used_l2_table *allocate_used_l2_table(
-	struct mc_instance	*instance
-) {
-	int				ret = 0;
-	struct mc_l2_table_store	*l2table_store = NULL;
-	struct mc_l2_tables_set		*l2table_set = NULL;
-	struct mc_used_l2_table		*used_l2table = NULL;
-	struct page			*page;
-	unsigned int			i = 0;
-
-	do {
-		/* allocate a WSM L2 descriptor */
-		used_l2table  = kmalloc(sizeof(*used_l2table), GFP_KERNEL);
-		if (used_l2table == NULL) {
-			ret = -ENOMEM;
-			MCDRV_DBG_ERROR("out of memory\n");
-			break;
-		}
-		/* clean */
-		memset(used_l2table, 0, sizeof(*used_l2table));
-		used_l2table->handle = get_mc_kmod_unique_id();
-		used_l2table->owner = instance;
-
-		/* add to global list. */
-		init_and_add_to_list(
-			&(used_l2table->list),
-			&(mc_drv_kmod_ctx.mc_used_l2_tables));
-
-		/* walk though list to find free set. */
-		list_for_each_entry(
-			l2table_set,
-			&(mc_drv_kmod_ctx.mc_l2_tables_sets),
-			list
-		) {
-			for (i = 0; i < MC_DRV_KMOD_L2_TABLE_PER_PAGES; i++) {
-				if ((l2table_set->usage_bitmap & (1U << i))
-					== 0) {
-					/* found a set,
-						l2table_set and i are set. */
-					l2table_store =
-						l2table_set->kernel_virt;
-					break;
-				}
-			}
-			if (l2table_store != NULL)
-				break;
-		} /* end while */
-
-		if (l2table_store == NULL) {
-			l2table_store = (struct mc_l2_table_store *)
-						get_zeroed_page(GFP_KERNEL);
-			if (l2table_store == NULL) {
-				ret = -ENOMEM;
-				break;
-			}
-
-			/* Actually, locking is not necessary, because kernel
-				memory is not supposed to get swapped out. But
-				we play safe.... */
-			page = virt_to_page(l2table_store);
-			SetPageReserved(page);
-
-			/* allocate a descriptor */
-			l2table_set = kmalloc(sizeof(*l2table_set), GFP_KERNEL);
-			if (l2table_set == NULL) {
-				kfree(l2table_store);
-				ret = -ENOMEM;
-				break;
-			}
-			/* initialize */
-			memset(l2table_set, 0, sizeof(*l2table_set));
-
-			l2table_set->kernel_virt = l2table_store;
-			l2table_set->page = page;
-			l2table_set->phys = (void *)virt_to_phys(l2table_store);
-
-			/* init add to list. */
-			init_and_add_to_list(
-				&(l2table_set->list),
-				&(mc_drv_kmod_ctx.mc_l2_tables_sets));
-
-			/* use first table */
-			i = 0;
-		}
-
-		/* set set usage */
-		l2table_set->usage_bitmap |= (1U << i);
-
-		/* set set reference */
-		used_l2table->set = l2table_set;
-		used_l2table->idx = i;
-
-		MCDRV_DBG_VERBOSE(
-			"chunkPhys=%p,idx=%d\n",
-			l2table_set->phys, i);
-
-	} while (FALSE);
-
-	if (ret != 0) {
-		if (used_l2table != NULL) {
-			/* remove from list */
-			list_del(&(l2table_set->list));
-			/* free memory */
-			kfree(used_l2table);
-			used_l2table = NULL;
-		}
-	}
-
-	return used_l2table;
-}
-
-/*----------------------------------------------------------------------------*/
-static void free_used_l2_table(
-	struct mc_used_l2_table *used_l2table
-)
-{
-	struct mc_l2_tables_set	*l2table_set;
-	unsigned int		idx;
-
-	MCDRV_ASSERT(used_l2table != NULL);
-
-	l2table_set = used_l2table->set;
-	MCDRV_ASSERT(l2table_set != NULL);
-
-	/* clean usage flag */
-	idx = used_l2table->idx;
-	MCDRV_ASSERT(idx < MC_DRV_KMOD_L2_TABLE_PER_PAGES);
-	l2table_set->usage_bitmap &= ~(1U << idx);
-
-	/* if nobody uses this set, we can release it. */
-	if (l2table_set->usage_bitmap == 0) {
-		MCDRV_ASSERT(l2table_set->page != NULL);
-		ClearPageReserved(l2table_set->page);
-
-		MCDRV_ASSERT(l2table_set->kernel_virt != NULL);
-		free_page((unsigned long)l2table_set->kernel_virt);
-
-		/* remove from list */
-		list_del(&(l2table_set->list));
-
-		/* free memory */
-		kfree(l2table_set);
-	}
-
-	return;
-}
-
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * Create a L2 table in a WSM container that has been allocates previously.
- *
- * @param task		pointer to task owning WSM
- * @param wsm_buffer	user space WSM start
- * @param wsm_len	WSM length
- * @param used_l2table	Pointer to L2 table details
- */
-static int map_buffer_into_used_l2_table(
-	struct task_struct	*task,
-	void			*wsm_buffer,
-	unsigned int		wsm_len,
-	struct mc_used_l2_table	*used_l2table
-)
-{
-	int		ret = 0;
-	unsigned int	i, nr_of_pages;
-	void		*virt_addr_page;
-	struct page	*page;
-	struct l2table	*l2table;
-	struct page	**l2table_as_array_of_pointers_to_page;
-
-	/* task can be null when called from kernel space */
-	MCDRV_ASSERT(wsm_buffer != NULL);
-	MCDRV_ASSERT(wsm_len != 0);
-	MCDRV_ASSERT(used_l2table != NULL);
-
-	MCDRV_DBG_VERBOSE("WSM addr=0x%p, len=0x%08x\n", wsm_buffer, wsm_len);
-
-	/* Check if called from kernel space wsm_buffer is actually
-	 * vmalloced or not */
-	if (task == NULL && !is_vmalloc_addr(wsm_buffer)) {
-		MCDRV_DBG_ERROR("WSM addr is not a vmalloc address");
-		return -EINVAL;
-	}
-
-	l2table = get_l2_table_kernel_virt(used_l2table);
-	/* We use the memory for the L2 table to hold the pointer
-	and convert them later. This works, as everything comes
-	down to a 32 bit value. */
-	l2table_as_array_of_pointers_to_page = (struct page **)l2table;
-
-	do {
-
-		/* no size > 1Mib supported */
-		if (wsm_len > SZ_1M) {
-			MCDRV_DBG_ERROR("size > 1 MiB\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		/* calculate page usage */
-		virt_addr_page = get_page_start(wsm_buffer);
-		nr_of_pages  = get_nr_of_pages_for_buffer(wsm_buffer, wsm_len);
-
-
-		MCDRV_DBG_VERBOSE("virt addr pageStart=0x%p,pages=%d\n",
-				  virt_addr_page,
-				  nr_of_pages);
-
-		/* L2 table can hold max 1MiB in 256 pages. */
-		if ((nr_of_pages*PAGE_SIZE) > SZ_1M) {
-			MCDRV_DBG_ERROR("WSM paged exceed 1 MiB\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		/* Request comes from user space */
-		if (task != NULL) {
-			/* lock user page in memory, so they do not get swapped
-			* out.
-			* REV axh:
-			* Kernel 2.6.27 added a new get_user_pages_fast()
-			* function, maybe it is called fast_gup() in some
-			* versions.
-			* handle user process doing a fork().
-			* Child should not get things.
-			* http://osdir.com/ml/linux-media/2009-07/msg00813.html
-			* http://lwn.net/Articles/275808/ */
-
-			ret = lock_user_pages(
-					  task,
-					  virt_addr_page,
-					  nr_of_pages,
-					  l2table_as_array_of_pointers_to_page);
-			if (ret != 0) {
-				MCDRV_DBG_ERROR("lock_user_pages() failed\n");
-				break;
-			}
-		}
-		/* Request comes from kernel space(vmalloc buffer) */
-		else {
-			void *uaddr = wsm_buffer;
-			for (i = 0; i < nr_of_pages; i++) {
-				page = vmalloc_to_page(uaddr);
-				if (!page) {
-					MCDRV_DBG_ERROR(
-						"vmalloc_to_Page()"
-						" failed to map address\n");
-					ret = -EINVAL;
-					break;
-				}
-				get_page(page);
-				/* Lock the page in memory, it can't be swapped
-				 * out */
-				SetPageReserved(page);
-				l2table_as_array_of_pointers_to_page[i] = page;
-				uaddr += PAGE_SIZE;
-			}
-		}
-
-		used_l2table->nr_of_pages = nr_of_pages;
-		used_l2table->flags |= MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_APP;
-
-		/* create L2 Table entries. used_l2table->table contains a list
-		of page pointers here. For a proper cleanup we have to ensure
-		that the following code either works and used_l2table contains
-		a valid L2 table - or fails and used_l2table->table contains the
-		list of page pointers. Any mixed contents will make cleanup
-		difficult.*/
-
-		for (i = 0; i < nr_of_pages; i++) {
-			pte_t pte;
-			page = l2table_as_array_of_pointers_to_page[i];
-
-			/* create L2 table entry, see ARM MMU docu for details
-			about flags stored in the lowest 12 bits. As a side
-			reference, the Article "ARM's multiply-mapped memory
-			mess" found in the collection at at
-			http://lwn.net/Articles/409032/ is also worth reading.*/
-			pte = page_to_l2_pte(page)
-					| L2_FLAG_AP1 | L2_FLAG_AP0
-					| L2_FLAG_C | L2_FLAG_B
-					| L2_FLAG_SMALL | L2_FLAG_SMALL_XN
-			/* Linux uses different mappings for SMP systems(the
-			 * sharing flag is set for the pte. In order not to
-			 * confuse things too much in Mobicore make sure the
-			 * shared buffers have the same flags.
-			 * This should also be done in SWD side
-			 */
-#ifdef CONFIG_SMP
-					| L2_FLAG_S | L2_FLAG_SMALL_TEX0
-#endif
-				  ;
-
-			l2table->table_entries[i] = pte;
-			MCDRV_DBG_VERBOSE("L2 entry %d:  0x%08x\n", i,
-					  (unsigned int)(pte));
-		}
-
-		/* ensure rest of table is empty */
-		while (i < 255)
-			l2table->table_entries[i++] = (pte_t)0;
-
-	} while (FALSE);
-
-	return ret;
-}
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * Remove a L2 table in a WSM container. Afterwards the container may be
- * released.
- *
- * @param used_l2table	Pointer to L2 table details
- */
-
-static void unmap_buffers_from_used_l2_table(
-	struct mc_used_l2_table	*used_l2table
-)
-{
-	unsigned int	i;
-	struct l2table	*l2table;
-
-	MCDRV_ASSERT(used_l2table != NULL);
-	/* this should not happen, as we have no empty tables. */
-	MCDRV_ASSERT(!is_in_use_used_l2_table(used_l2table));
-
-	/* found the table, now release the resources. */
-	MCDRV_DBG_VERBOSE("clear L2 table, phys_base=%p, nr_of_pages=%d\n",
-			  get_l2_table_phys(used_l2table),
-			  used_l2table->nr_of_pages);
-
-	l2table = get_l2_table_kernel_virt(used_l2table);
-
-	/* release all locked user space pages */
-	for (i = 0; i < used_l2table->nr_of_pages; i++) {
-		/* convert physical entries from L2 table to page pointers */
-		pte_t pte = get_l2_table_kernel_virt(used_l2table)->
-							table_entries[i];
-		struct page *page = l2_pte_to_page(pte);
-		unlock_page_from_used_l2_table(page);
-	}
-
-	/* remember that all pages have been freed */
-	used_l2table->nr_of_pages = 0;
-
-	return;
-}
-
-
-/*
-#############################################################################
-##
-## Helper functions
-##
-#############################################################################*/
-/*----------------------------------------------------------------------------*/
-#define FREE_FROM_SWD	TRUE
-#define FREE_FROM_NWD	FALSE
-/** Delete a used l2 table. */
-static void delete_used_l2_table(
-	struct mc_used_l2_table	*used_l2table,
-	unsigned int		is_swd
-)
-{
-	if (is_swd) {
-		used_l2table->flags &=
-			~MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-	} else {
-		used_l2table->flags &=
-			~MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_APP;
-		used_l2table->owner = NULL;
-	}
-
-	/* release if Nwd and Swd/MC do no longer use it. */
-	if (is_in_use_used_l2_table(used_l2table)) {
-		MCDRV_DBG_WARN(
-			"WSM L2 table still in use: physBase=%p, "
-			"nr_of_pages=%d\n",
-			get_l2_table_phys(used_l2table),
-			used_l2table->nr_of_pages);
-	} else {
-		unmap_buffers_from_used_l2_table(used_l2table);
-		free_used_l2_table(used_l2table);
-
-		list_del(&(used_l2table->list));
-
-		kfree(used_l2table);
-	}
-	return;
-}
-
-/*----------------------------------------------------------------------------*/
-/** Allocate L2 table and map buffer into it. That is, create respective table
-	entries. Must hold Semaphore mc_drv_kmod_ctx.wsm_l2_sem */
-static struct mc_used_l2_table *new_used_l2_table(
-	struct mc_instance	*instance,
-	struct task_struct	*task,
-	void			*wsm_buffer,
-	unsigned int		wsm_len
-) {
-	int			ret = 0;
-	struct mc_used_l2_table	*used_l2table;
-
-	do {
-		used_l2table = allocate_used_l2_table(instance);
-		if (used_l2table == NULL) {
-			MCDRV_DBG_ERROR(
-				"allocate_used_l2_table() failed\n");
-			break;
-		}
-
-		/* create the L2 page for the WSM */
-		ret = map_buffer_into_used_l2_table(
-				  task,
-				  wsm_buffer,
-				  wsm_len,
-				  used_l2table);
-		if (ret != 0) {
-			MCDRV_DBG_ERROR(
-				"map_buffer_into_used_l2_table() failed\n");
-			delete_used_l2_table(used_l2table, FREE_FROM_NWD);
-			used_l2table = NULL;
-			break;
-		}
-
-	} while (FALSE);
-
-
-	return used_l2table;
-}
-
-/*
-#############################################################################
-##
-## IoCtl handler
-##
-#############################################################################*/
-
-/**
- * Map a virtual memory buffer structure to Mobicore
- * @param instance
- * @param addr		address of the buffer(NB it must be kernel virtual!)
- * @param len		buffer length
- * @param handle	pointer to handle
- * @param phys_wsm_l2_table	pointer to physical L2 table(?)
- *
- * @return 0 if no error
- *
- */
-/*----------------------------------------------------------------------------*/
-int mobicore_map_vmem(
-	struct mc_instance	*instance,
-	void			*addr,
-	uint32_t		len,
-	uint32_t		*handle,
-	void			**phys_wsm_l2_table
-)
-{
-	int ret = 0;
-	struct mc_used_l2_table *used_l2table = NULL;
-	MCDRV_ASSERT(instance != NULL);
-
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		if (len == 0) {
-			MCDRV_DBG_ERROR("len=0 is not supported!\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("down_interruptible() failed with %d\n",
-					ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			used_l2table = new_used_l2_table(
-						   instance,
-						   NULL,
-						   addr,
-						   len);
-
-			if (used_l2table == NULL) {
-				MCDRV_DBG_ERROR("new_used_l2_table() failed\n");
-				ret = -EINVAL;
-				break;
-			}
-
-			/* set response */
-			*handle = used_l2table->handle;
-			*phys_wsm_l2_table =
-				(void *)get_l2_table_phys(used_l2table);
-			MCDRV_DBG_VERBOSE("handle: %d, phys=%p\n",
-					  *handle,
-					  (void *)(*phys_wsm_l2_table));
-
-		} while (FALSE);
-
-		/* release semaphore */
-		up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-EXPORT_SYMBOL(mobicore_map_vmem);
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_app_register_wsm_l2(
-	struct mc_instance			*instance,
-	union mc_ioctl_app_reg_wsm_l2_params	*user_params
-)
-{
-	int					ret = 0;
-	union mc_ioctl_app_reg_wsm_l2_params	params;
-	struct mc_used_l2_table			*used_l2table = NULL;
-	struct pid				*pid_struct = NULL;
-	struct task_struct			*task = current;
-
-	MCDRV_ASSERT(instance != NULL);
-
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* get use parameters */
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user() failed\n");
-			break;
-		}
-
-		/* daemon can do this for another task. */
-		if (params.in.pid != 0) {
-			MCDRV_DBG_ERROR("pid != 0 unsupported\n");
-			ret = -EINVAL;
-			break;
-		}
-		if (params.in.len == 0) {
-			MCDRV_DBG_ERROR("len=0 is not supported!\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("down_interruptible() failed with %d\n",
-					ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			used_l2table = new_used_l2_table(
-						   instance,
-						   task,
-						   (void *)(params.in.buffer),
-						   params.in.len);
-
-			if (used_l2table == NULL) {
-				MCDRV_DBG_ERROR("new_used_l2_table() failed\n");
-				ret = -EINVAL;
-				break;
-			}
-
-			/* if the daemon does this, we set the MC lock */
-			if (is_caller_mc_daemon(instance))
-				used_l2table->flags |=
-					MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-
-			/* set response */
-			memset(&params.out, 0, sizeof(params.out));
-			params.out.handle = used_l2table->handle;
-			/* TODO: return the physical address for daemon only,
-				otherwise set NULL */
-			params.out.phys_wsm_l2_table =
-				(uint32_t)get_l2_table_phys(used_l2table);
-
-			MCDRV_DBG_VERBOSE("handle: %d, phys=%p\n",
-					params.out.handle,
-					(void *)(params.out.phys_wsm_l2_table));
-
-
-			/* copy L2Table to user space */
-			ret = copy_to_user(
-					  &(user_params->out),
-					  &(params.out),
-					  sizeof(params.out));
-			if (ret != 0) {
-				MCDRV_DBG_ERROR("copy_to_user() failed\n");
-
-				/* free the table again, as app does not know
-					about anything. */
-				if (is_caller_mc_daemon(instance)) {
-					used_l2table->flags &=
-					~MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-				}
-				delete_used_l2_table(used_l2table,
-						FREE_FROM_NWD);
-				used_l2table = NULL;
-				break;
-			}
-
-		} while (FALSE);
-
-		/* release semaphore */
-		up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-
-	} while (FALSE);
-
-
-
-	/* release PID struct reference */
-	if (pid_struct != NULL)
-		put_pid(pid_struct);
-
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * Unmap a virtual memory buffer from mobicore
- * @param instance
- * @param handle
- *
- * @return 0 if no error
- *
- */
-int mobicore_unmap_vmem(
-	struct mc_instance	*instance,
-	uint32_t		handle
-)
-{
-	int ret = 0;
-	struct mc_used_l2_table *used_l2table = NULL;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("processOpenSession() failed with %d\n",
-					ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			used_l2table = find_used_l2_table_by_handle(handle);
-			if (used_l2table == NULL) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("entry not found\n");
-				break;
-			}
-
-			if (instance != used_l2table->owner) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("instance does no own it\n");
-				break;
-			}
-
-			/* free table (if no further locks exist) */
-			delete_used_l2_table(used_l2table, FREE_FROM_NWD);
-			used_l2table = NULL;
-			/* there are no out parameters */
-		} while (FALSE);
-		/* release semaphore */
-		up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-EXPORT_SYMBOL(mobicore_unmap_vmem);
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_app_unregister_wsm_l2(
-	struct mc_instance			*instance,
-	struct mc_ioctl_app_unreg_wsm_l2_params	*user_params
-)
-{
-	int					ret = 0;
-	struct mc_ioctl_app_unreg_wsm_l2_params	params;
-	struct mc_used_l2_table			*used_l2table = NULL;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user\n");
-			break;
-		}
-
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("down_interruptible() failed with %d\n",
-					ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			/* daemon can do this for another task. */
-			if (params.in.pid != 0) {
-				MCDRV_DBG_ERROR("pid != 0 unsupported\n");
-				ret = -EINVAL;
-				break;
-			}
-
-			used_l2table =
-				find_used_l2_table_by_handle(params.in.handle);
-			if (used_l2table == NULL) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("entry not found\n");
-				break;
-			}
-
-			if (is_caller_mc_daemon(instance)) {
-				/* if daemon does this, we have to release the
-					MobiCore lock. */
-				used_l2table->flags &=
-					~MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-			} else if (instance != used_l2table->owner) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("instance does no own it\n");
-				break;
-			}
-
-			/* free table (if no further locks exist) */
-			delete_used_l2_table(used_l2table, FREE_FROM_NWD);
-			used_l2table = NULL;
-
-			/* there are no out parameters */
-
-		} while (FALSE);
-
-		/* release semaphore */
-		up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-
-/*----------------------------------------------------------------------------*/
-static int handle_ioctl_daemon_lock_wsm_l2(
-	struct mc_instance				*instance,
-	struct mc_ioctl_daemon_lock_wsm_l2_params	*user_params
-)
-{
-	int						ret = 0;
-	struct mc_ioctl_daemon_lock_wsm_l2_params	params;
-	struct mc_used_l2_table				*used_l2table = NULL;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user\n");
-			break;
-		}
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("down_interruptible() failed with %d\n",
-					ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			used_l2table =
-				find_used_l2_table_by_handle(params.in.handle);
-			if (used_l2table == NULL) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("entry not found\n");
-				break;
-			}
-			if (instance != used_l2table->owner) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("instance does no own it\n");
-				break;
-			}
-
-			/* lock entry */
-			if ((used_l2table->flags &
-				  MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC) != 0) {
-				MCDRV_DBG_WARN("entry already locked\n");
-			}
-			used_l2table->flags |=
-				MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-
-			/* prepare response */
-			memset(&(params.out), 0, sizeof(params.out));
-			params.out.phys_wsm_l2_table =
-				(uint32_t)get_l2_table_phys(used_l2table);
-
-			/* copy to user space */
-			ret = copy_to_user(
-					  &(user_params->out),
-					  &(params.out),
-					  sizeof(params.out));
-			if (ret != 0) {
-				MCDRV_DBG_ERROR("copy_to_user() failed\n");
-
-				/* undo, as userspace did not get it. */
-				used_l2table->flags |=
-					MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC;
-				break;
-			}
-
-		} while (FALSE);
-
-		/* release semaphore */
-		up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-
-/*----------------------------------------------------------------------------*/
-static int handle_ioctl_daemon_unlock_wsm_l2(
-	struct mc_instance				*instance,
-	struct mc_ioctl_daemon_unlock_wsm_l2_params	*user_params
-)
-{
-	int						ret = 0;
-	struct mc_ioctl_daemon_unlock_wsm_l2_params	params;
-	struct mc_used_l2_table				*used_l2table = NULL;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user\n");
-			break;
-		}
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("down_interruptible() failed with %d\n",
-						ret);
-			ret = -ERESTARTSYS;
-			break;
-		}
-
-		do {
-			used_l2table =
-				find_used_l2_table_by_handle(params.in.handle);
-			if (used_l2table == NULL) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("entry not found\n");
-				break;
-			}
-			if (instance != used_l2table->owner) {
-				ret = -EINVAL;
-				MCDRV_DBG_ERROR("instance does no own it\n");
-				break;
-			}
-
-			/* lock entry */
-			if ((used_l2table->flags &
-				  MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC) == 0) {
-				MCDRV_DBG_WARN("entry is not locked locked\n");
-			}
-
-			/* free table (if no further locks exist) */
-			delete_used_l2_table(used_l2table, FREE_FROM_SWD);
-			used_l2table = NULL;
-
-			/* there are no out parameters */
-
-		} while (FALSE);
-
-	} while (FALSE);
-
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/** Clears the reserved bit of each page and frees the pages */
-static inline void free_continguous_pages(
-	void		*addr,
-	unsigned int	size
-)
-{
-	struct page *page = virt_to_page(addr);
-	int i;
-	for (i = 0; i < size; i++) {
-		MCDRV_DBG_VERBOSE("free page at 0x%p\n", page);
-		ClearPageReserved(page);
-		page++;
-	}
-	/* REV luh: see man kmalloc */
-	free_pages((unsigned long)addr, size_to_order(size));
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- * Free a WSM buffer allocated with mobicore_allocate_wsm
- * @param instance
- * @param handle		handle of the buffer
- *
- * @return 0 if no error
- *
- */
-int mobicore_free(
-	struct mc_instance	*instance,
-	uint32_t		handle
-)
-{
-	int ret = 0;
-	unsigned int i;
-	struct mc_contg_buffer	*contg_buffer;
-
-	do {
-		/* search for the given address in the contg_buffers list */
-		for (i = 0; i < MC_DRV_KMOD_CONTG_BUFFER_MAX; i++) {
-			contg_buffer = &(instance->contg_buffers[i]);
-			if (contg_buffer->handle == handle)
-				break;
-		}
-		if (i == MC_DRV_KMOD_CONTG_BUFFER_MAX) {
-			MCDRV_DBG_ERROR("contigous buffer not found\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		MCDRV_DBG_VERBOSE("phys_addr=0x%p, virt_addr=0x%p\n",
-				contg_buffer->phys_addr,
-				contg_buffer->virt_kernel_addr);
-
-		free_continguous_pages(contg_buffer->virt_kernel_addr,
-					contg_buffer->num_pages);
-
-		memset(contg_buffer, 0, sizeof(*contg_buffer));
-
-		/* there are no out parameters */
-
-	} while (FALSE);
-
-
-	return ret;
-}
-EXPORT_SYMBOL(mobicore_free);
-/*----------------------------------------------------------------------------*/
-
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_free(
-	struct mc_instance		*instance,
-	union mc_ioctl_free_params	*user_params
-)
-{
-	int				ret = 0;
-	union mc_ioctl_free_params	params;
-
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user\n");
-			break;
-		}
-
-		/* daemon can do this for another task. */
-		if (params.in.pid != 0) {
-			MCDRV_DBG_ERROR("pid != 0 unsupported\n");
-			ret = -EINVAL;
-			break;
-		}
-
-		ret = mobicore_free(instance, params.in.handle);
-
-		/* there are no out parameters */
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-
-}
-
-
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_info(
-	struct mc_instance	*instance,
-	union mc_ioctl_info_params	*user_params
-)
-{
-	int			ret = 0;
-	union mc_ioctl_info_params	params;
-	union mc_fc_info		fc_info;
-
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* only the MobiCore Daemon is allowed to call this function */
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user\n");
-			break;
-		}
-
-
-		memset(&fc_info, 0, sizeof(fc_info));
-		fc_info.as_in.cmd	   = MC_FC_INFO;
-		fc_info.as_in.ext_info_id = params.in.ext_info_id;
-
-		MCDRV_DBG(
-			"fc_info in cmd=0x%08x, ext_info_id=0x%08x "
-			"rfu=(0x%08x, 0x%08x)\n",
-			fc_info.as_in.cmd,
-			fc_info.as_in.ext_info_id,
-			fc_info.as_in.rfu[0],
-			fc_info.as_in.rfu[1]);
-
-		mc_fastcall(&(fc_info.as_generic));
-
-		MCDRV_DBG(
-			"fc_info out resp=0x%08x, ret=0x%08x "
-			"state=0x%08x, ext_info=0x%08x\n",
-			fc_info.as_out.resp,
-			fc_info.as_out.ret,
-			fc_info.as_out.state,
-			fc_info.as_out.ext_info);
-
-		ret = convert_fc_ret(fc_info.as_out.ret);
-		if (ret != 0)
-			break;
-
-		memset(&(params.out), 0, sizeof(params.out));
-		params.out.state  = fc_info.as_out.state;
-		params.out.ext_info = fc_info.as_out.ext_info;
-
-		ret = copy_to_user(
-				  &(user_params->out),
-				  &(params.out),
-				  sizeof(params.out));
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_to_user\n");
-			break;
-		}
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_yield(
-	struct mc_instance	*instance
-)
-{
-	int			ret = 0;
-	union mc_fc_s_yield	fc_s_yield;
-
-	MCDRV_ASSERT(instance != NULL);
-
-	/* avoid putting debug output here, as we do this very often */
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* only the MobiCore Daemon is allowed to call this function */
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		memset(&fc_s_yield, 0, sizeof(fc_s_yield));
-		fc_s_yield.as_in.cmd = MC_SMC_N_YIELD;
-		mc_fastcall(&(fc_s_yield.as_generic));
-		ret = convert_fc_ret(fc_s_yield.as_out.ret);
-		if (ret != 0)
-			break;
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- * handle ioctl and call common notify
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_nsiq(
-	struct mc_instance	*instance,
-	unsigned long		arg
-)
-{
-	int		ret = 0;
-
-	MCDRV_ASSERT(instance != NULL);
-
-	/* avoid putting debug output here, as we do this very often */
-	MCDRV_DBG_VERBOSE("enter\n");
-	/* only the MobiCore Daemon is allowed to call this function */
-	if (!is_caller_mc_daemon(instance)) {
-		MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-		return -EFAULT;
-	}
-
-	do {
-		union mc_fc_nsiq fc_nsiq;
-		memset(&fc_nsiq, 0, sizeof(fc_nsiq));
-		fc_nsiq.as_in.cmd = MC_SMC_N_SIQ;
-		mc_fastcall(&(fc_nsiq.as_generic));
-		ret = convert_fc_ret(fc_nsiq.as_out.ret);
-		if (ret != 0)
-			break;
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_dump_status(
-	struct mc_instance	*instance,
-	unsigned long		arg
-)
-{
-	int		ret = 0;
-	int		i = 0;
-	union mc_fc_info	fc_info;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* anybody with root access can do this. */
-		if (!is_userland_caller_privileged()) {
-			MCDRV_DBG_ERROR("caller must have root privileges\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		/* loop ext_info */
-		while (TRUE) {
-			memset(&fc_info, 0, sizeof(fc_info));
-			fc_info.as_in.cmd	   = MC_FC_INFO;
-			fc_info.as_in.ext_info_id = i;
-
-			MCDRV_DBG(
-				"fc_info in cmd=0x%08x, ext_info_id=0x%08x "
-				"rfu=(0x%08x, 0x%08x)\n",
-				fc_info.as_in.cmd,
-				fc_info.as_in.ext_info_id,
-				fc_info.as_in.rfu[0],
-				fc_info.as_in.rfu[1]);
-
-			mc_fastcall(&(fc_info.as_generic));
-
-			MCDRV_DBG(
-				"fc_info out resp=0x%08x, ret=0x%08x "
-				"state=0x%08x, ext_info=0x%08x\n",
-				fc_info.as_out.resp,
-				fc_info.as_out.ret,
-				fc_info.as_out.state,
-				fc_info.as_out.ext_info);
-
-			ret = convert_fc_ret(fc_info.as_out.ret);
-			if (ret != 0)
-				break;
-
-			MCDRV_DBG("state=%08X, idx=%02d: ext_info=%08X\n",
-				fc_info.as_out.state,
-				i,
-				fc_info.as_out.ext_info);
-			i++;
-		};
-
-		if (ret != 0)
-			break;
-
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_init(
-	struct mc_instance	*instance,
-	union mc_ioctl_init_params	*user_params
-)
-{
-	int			ret = 0;
-	union mc_ioctl_init_params	params;
-	union mc_fc_init		fc_init;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* only the MobiCore Daemon is allowed to call this function */
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user failed\n");
-			break;
-		}
-
-		memset(&fc_init, 0, sizeof(fc_init));
-
-		fc_init.as_in.cmd	= MC_FC_INIT;
-		/* base address of mci buffer 4KB aligned */
-		fc_init.as_in.base   = (uint32_t)params.in.base;
-		/* notification buffer start/length [16:16] [start, length] */
-		fc_init.as_in.nq_info  = (params.in.nq_offset << 16)
-					  | (params.in.nq_length & 0xFFFF);
-		/* mcp buffer start/length [16:16] [start, length] */
-		fc_init.as_in.mcp_info = (params.in.mcp_offset << 16)
-					  | (params.in.mcp_length & 0xFFFF);
-
-		/* Set KMOD notification queue to start of MCI
-			mciInfo was already set up in mmap */
-		if (!mci_base) {
-			MCDRV_DBG_ERROR("No MCI set yet.\n");
-			return -EFAULT;
-		}
-		MCDRV_DBG("in cmd=0x%08x, base=0x%08x, "
-			  "nq_info=0x%08x, mcp_info=0x%08x\n",
-			  fc_init.as_in.cmd,
-			  fc_init.as_in.base,
-			  fc_init.as_in.nq_info,
-			  fc_init.as_in.mcp_info);
-
-		mc_fastcall(&(fc_init.as_generic));
-
-		MCDRV_DBG("out cmd=0x%08x, ret=0x%08x rfu=(0x%08x, 0x%08x)\n",
-			  fc_init.as_out.resp,
-			  fc_init.as_out.ret,
-			  fc_init.as_out.rfu[0],
-			  fc_init.as_out.rfu[1]);
-
-		ret = convert_fc_ret(fc_init.as_out.ret);
-		if (ret != 0)
-			break;
-
-		/* no ioctl response parameters */
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_fc_execute(
-	struct mc_instance		*instance,
-	union mc_ioctl_fc_execute_params	*user_params
-)
-{
-	int				ret = 0;
-	union mc_ioctl_fc_execute_params	params;
-	union fc_generic			fc_params;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* only the MobiCore Daemon is allowed to call this function */
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		ret = copy_from_user(
-				  &(params.in),
-				  &(user_params->in),
-				  sizeof(params.in));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_from_user failed\n");
-			break;
-		}
-
-		fc_params.as_in.cmd = -4;/*FC_EXECUTE */
-		fc_params.as_in.param[0] = params.in.phys_start_addr;
-		fc_params.as_in.param[1] = params.in.length;
-		fc_params.as_in.param[2] = 0;
-
-		MCDRV_DBG("in cmd=0x%08x, startAddr=0x%08x, length=0x%08x\n",
-			  fc_params.as_in.cmd,
-			  fc_params.as_in.param[0],
-			  fc_params.as_in.param[1]);
-
-		mc_fastcall(&fc_params);
-
-		MCDRV_DBG("out cmd=0x%08x, ret=0x%08x rfu=(0x%08x, 0x%08x)\n",
-			  fc_params.as_out.resp,
-			  fc_params.as_out.ret,
-			  fc_params.as_out.param[0],
-			  fc_params.as_out.param[1]);
-
-		ret = convert_fc_ret(fc_params.as_out.ret);
-		if (ret != 0)
-			break;
-
-		/* no ioctl response parameters */
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-#define MC_MAKE_VERSION(major, minor) \
-		(((major & 0x0000ffff) << 16) | (minor & 0x0000ffff))
-/**
- *
- * @param instance
- * @param arg
- *
- * @return 0 if no error
- *
- */
-static int handle_ioctl_get_version(
-	struct mc_instance			*instance,
-	struct mc_ioctl_get_version_params	*user_params
-)
-{
-	int ret = 0;
-	struct mc_ioctl_get_version_params params = {
-		{
-			MC_MAKE_VERSION(MCDRVMODULEAPI_VERSION_MAJOR,
-					MCDRVMODULEAPI_VERSION_MINOR)
-		}
-	};
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		MCDRV_DBG("mcDrvModuleApi version is %i.%i\n",
-				MCDRVMODULEAPI_VERSION_MAJOR,
-				MCDRVMODULEAPI_VERSION_MINOR);
-
-		/* no ioctl response parameters */
-		ret = copy_to_user(
-					&(user_params->out),
-					&(params.out),
-					sizeof(params.out));
-		if (ret != 0)
-			MCDRV_DBG_ERROR("copy_to_user() failed\n");
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- * This function will be called from user space as ioctl(...).
- * @param file	pointer to file
- * @param cmd	command
- * @param arg	arguments
- *
- * @return int 0 for OK and an errno in case of error
- */
-static long mc_kernel_module_ioctl(
-	struct file	*file,
-	unsigned int	cmd,
-	unsigned long	arg
-)
-{
-	int ret;
 	struct mc_instance *instance = get_instance(file);
+	unsigned long len = vmarea->vm_end - vmarea->vm_start;
+	void *paddr = (void *)(vmarea->vm_pgoff << PAGE_SHIFT);
+	unsigned int pfn;
+	struct mc_buffer *buffer = 0;
+	int ret = 0;
 
-	MCDRV_ASSERT(instance != NULL);
+	MCDRV_DBG(mcd, "enter (vma start=0x%p, size=%ld, mci=%p)\n",
+		  (void *)vmarea->vm_start, len, ctx.mci_base.phys);
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (len == 0) {
+		MCDRV_DBG_ERROR(mcd, "cannot allocate size 0\n");
+		return -ENOMEM;
+	}
+	if (paddr) {
+		mutex_lock(&ctx.bufs_lock);
+
+		/* search for the buffer list. */
+		list_for_each_entry(buffer, &ctx.cont_bufs, list) {
+			if (buffer->phys == paddr)
+				goto found;
+			else
+					break;
+		}
+		/* Nothing found return */
+		mutex_unlock(&ctx.bufs_lock);
+		return -EINVAL;
+
+found:
+		vmarea->vm_flags |= VM_RESERVED;
+		/*
+		 * Convert kernel address to user address. Kernel address begins
+		 * at PAGE_OFFSET, user address range is below PAGE_OFFSET.
+		 * Remapping the area is always done, so multiple mappings
+		 * of one region are possible. Now remap kernel address
+		 * space into user space
+		 */
+		pfn = (unsigned int)paddr >> PAGE_SHIFT;
+		ret = (int)remap_pfn_range(vmarea, vmarea->vm_start, pfn,
+			buffer->len, vmarea->vm_page_prot);
+		mutex_unlock(&ctx.bufs_lock);
+	} else {
+		if (!is_daemon(instance))
+			return -EPERM;
+
+		paddr = get_mci_base_phys(len);
+		if (!paddr)
+			return -EFAULT;
+
+		vmarea->vm_flags |= VM_RESERVED;
+		/*
+		 * Convert kernel address to user address. Kernel address begins
+		 * at PAGE_OFFSET, user address range is below PAGE_OFFSET.
+		 * Remapping the area is always done, so multiple mappings
+		 * of one region are possible. Now remap kernel address
+		 * space into user space
+		 */
+		pfn = (unsigned int)paddr >> PAGE_SHIFT;
+		ret = (int)remap_pfn_range(vmarea, vmarea->vm_start, pfn, len,
+			vmarea->vm_page_prot);
+	}
+
+	MCDRV_DBG_VERBOSE(mcd, "exit with %d/0x%08X\n", ret, ret);
+
+	return ret;
+}
+
+static inline int ioctl_check_pointer(unsigned int cmd, int __user *uarg)
+{
+	int err = 0;
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		err = !access_ok(VERIFY_WRITE, uarg, _IOC_SIZE(cmd));
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		err = !access_ok(VERIFY_READ, uarg, _IOC_SIZE(cmd));
+	if (err)
+		return -EFAULT;
+
+	return 0;
+}
+
+/*
+ * mc_fd_user_ioctl() - Will be called from user space as ioctl(..)
+ * @file	pointer to file
+ * @cmd		command
+ * @arg		arguments
+ *
+ * Returns 0 for OK and an errno in case of error
+ */
+static long mc_fd_user_ioctl(struct file *file, unsigned int cmd,
+	unsigned long arg)
+{
+	struct mc_instance *instance = get_instance(file);
+	int __user *uarg = (int __user *)arg;
+	int ret = -EINVAL;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	if (ioctl_check_pointer(cmd, uarg))
+		return -EFAULT;
 
 	switch (cmd) {
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_DUMP_STATUS:
-		ret = handle_ioctl_dump_status(
-				instance,
-				arg);
+	case MC_IO_FREE:
+		ret = mc_free_buffer(instance, (uint32_t)arg);
 		break;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FC_INIT:
-		ret = handle_ioctl_init(
-				instance,
-				(union mc_ioctl_init_params *)arg);
+	case MC_IO_REG_WSM:{
+		struct mc_ioctl_reg_wsm reg;
+		if (copy_from_user(&reg, uarg, sizeof(reg)))
+			return -EFAULT;
+
+		ret = mc_register_wsm_l2(instance, reg.buffer,
+			reg.len, &reg.handle, &reg.table_phys);
+		if (!ret) {
+			if (copy_to_user(uarg, &reg, sizeof(reg))) {
+				ret = -EFAULT;
+				mc_unregister_wsm_l2(instance, reg.handle);
+			}
+		}
 		break;
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FC_INFO:
-		ret = handle_ioctl_info(
-				instance,
-				(union mc_ioctl_info_params *)arg);
+	}
+	case MC_IO_UNREG_WSM:
+		ret = mc_unregister_wsm_l2(instance, (uint32_t)arg);
 		break;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FC_YIELD:
-		ret = handle_ioctl_yield(
-				instance);
+	case MC_IO_VERSION:
+		ret = put_user(mc_get_version(), uarg);
+		if (ret)
+			MCDRV_DBG_ERROR(mcd,
+					"IOCTL_GET_VERSION failed to put data");
 		break;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FC_NSIQ:
-		ret = handle_ioctl_nsiq(
-				instance,
-				arg);
-		break;
+	case MC_IO_MAP_WSM:{
+		struct mc_ioctl_map map;
+		struct mc_buffer *buffer = 0;
+		if (copy_from_user(&map, uarg, sizeof(map)))
+			return -EFAULT;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_DAEMON_LOCK_WSM_L2:
-		ret = handle_ioctl_daemon_lock_wsm_l2(
-			instance,
-			(struct mc_ioctl_daemon_lock_wsm_l2_params *)arg);
-		break;
+		/* Setup the WSM buffer structure! */
+		if (mc_get_buffer(instance, &buffer, map.len))
+			return -EFAULT;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_DAEMON_UNLOCK_WSM_L2:
-		ret = handle_ioctl_daemon_unlock_wsm_l2(
-			instance,
-			(struct mc_ioctl_daemon_unlock_wsm_l2_params *)arg);
-		break;
+		map.handle = buffer->handle;
+		map.phys_addr = (unsigned long)buffer->phys;
+		map.reused = 0;
+		if (copy_to_user(uarg, &map, sizeof(map)))
+			ret = -EFAULT;
 
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FREE:
-		/* called by ClientLib */
-		ret = handle_ioctl_free(
-				instance,
-				(union mc_ioctl_free_params *)arg);
+		ret = 0;
 		break;
-
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_APP_REGISTER_WSM_L2:
-		/* called by ClientLib */
-		ret = handle_ioctl_app_register_wsm_l2(
-				instance,
-				(union mc_ioctl_app_reg_wsm_l2_params *)arg);
-		break;
-
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_APP_UNREGISTER_WSM_L2:
-		/* called by ClientLib */
-		ret = handle_ioctl_app_unregister_wsm_l2(
-				instance,
-				(struct mc_ioctl_app_unreg_wsm_l2_params *)arg);
-		break;
-
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_FC_EXECUTE:
-		ret = handle_ioctl_fc_execute(
-				instance,
-				(union mc_ioctl_fc_execute_params *)arg);
-		break;
-
-	/*--------------------------------------------------------------------*/
-	case MC_DRV_KMOD_IOCTL_GET_VERSION:
-		ret = handle_ioctl_get_version(
-				instance,
-				(struct mc_ioctl_get_version_params *)arg);
-		break;
-
-	/*--------------------------------------------------------------------*/
+	}
 	default:
-		MCDRV_DBG_ERROR("unsupported cmd=%d\n", cmd);
-		ret = -EFAULT;
+		MCDRV_DBG_ERROR(mcd, "unsupported cmd=%d\n", cmd);
+		ret = -ENOIOCTLCMD;
 		break;
 
 	} /* end switch(cmd) */
@@ -1975,895 +632,533 @@
 	return (int)ret;
 }
 
-
-/*----------------------------------------------------------------------------*/
-/**
- * This function will be called from user space as read(...).
- * The read function is blocking until a interrupt occurs. In that case the
- * event counter is copied into user space and the function is finished.
- * @param *file
- * @param *buffer  buffer where to copy to(userspace)
- * @param buffer_len	 number of requested data
- * @param *pos	 not used
- * @return ssize_t  ok case: number of copied data
- *				error case: return errno
- */
-static ssize_t mc_kernel_module_read(
-	struct file	*file,
-	char		*buffer,
-	size_t		buffer_len,
-	loff_t		*pos
-)
+static long mc_fd_admin_ioctl(struct file *file, unsigned int cmd,
+	unsigned long arg)
 {
-	int ret = 0, ssiq_counter;
-	size_t retLen = 0;
 	struct mc_instance *instance = get_instance(file);
+	int __user *uarg = (int __user *)arg;
+	int ret = -EINVAL;
 
-	MCDRV_ASSERT(instance != NULL);
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
 
-	/* avoid debug output on non-error, because this is call quite often */
-	MCDRV_DBG_VERBOSE("enter\n");
-
-	do {
-		/* only the MobiCore Daemon is allowed to call this function */
-		if (!is_caller_mc_daemon(instance)) {
-			MCDRV_DBG_ERROR("caller not MobiCore Daemon\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		if (buffer_len < sizeof(unsigned int)) {
-			MCDRV_DBG_ERROR("invalid length\n");
-			ret = (ssize_t)(-EINVAL);
-			break;
-		}
-
-		for (;;) {
-			if (down_interruptible(
-					&mc_drv_kmod_ctx.daemon_ctx.sem)) {
-				MCDRV_DBG_VERBOSE("read interrupted\n");
-				ret = (ssize_t)-ERESTARTSYS;
-				break;
-			}
-
-			ssiq_counter = atomic_read(
-					&(mc_drv_kmod_ctx.ssiq_ctx.counter));
-			MCDRV_DBG_VERBOSE("ssiq_counter=%i, ctx.counter=%i\n",
-				ssiq_counter,
-				mc_drv_kmod_ctx.daemon_ctx.ssiq_counter);
-
-			if (ssiq_counter !=
-				mc_drv_kmod_ctx.daemon_ctx.ssiq_counter) {
-				/* read data and exit loop without
-					error */
-				mc_drv_kmod_ctx.daemon_ctx.ssiq_counter =
-					ssiq_counter;
-				ret = 0;
-				break;
-			}
-
-			/* end loop if non-blocking */
-			if ((file->f_flags & O_NONBLOCK) != 0) {
-				MCDRV_DBG_ERROR("non-blocking read\n");
-				ret = (ssize_t)(-EAGAIN);
-				break;
-			}
-
-			if (signal_pending(current) != 0) {
-				MCDRV_DBG_VERBOSE("received signal.\n");
-				ret = (ssize_t)(-ERESTARTSYS);
-				break;
-			}
-
-		}
-
-		/* we are here if an event occurred or we had an
-			error.*/
-		if (ret != 0)
-			break;
-
-		/* read data and exit loop */
-		ret = copy_to_user(
-				  buffer,
-				  &(mc_drv_kmod_ctx.daemon_ctx.ssiq_counter),
-				  sizeof(unsigned int));
-
-
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("copy_to_user failed\n");
-			ret = (ssize_t)(-EFAULT);
-			break;
-		}
-
-		retLen = sizeof(s32);
-
-	} while (FALSE);
-
-	/* avoid debug on non-error. */
-	if (ret == 0)
-		ret = (size_t)retLen;
-	else
-		MCDRV_DBG("exit with %d/0x%08X\n", ret, ret);
-
-	return (ssize_t)ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- * Allocate WSM for given instance
- *
- * @param instance		instance
- * @param requested_size		size of the WSM
- * @param handle		pointer where the handle will be saved
- * @param virt_kernel_addr	pointer for the kernel virtual address
- * @param phys_addr		pointer for the physical address
- *
- * @return error code or 0 for success
- */
-int mobicore_allocate_wsm(
-	struct mc_instance	*instance,
-	unsigned long		requested_size,
-	uint32_t		*handle,
-	void			**virt_kernel_addr,
-	void			**phys_addr
-)
-{
-	unsigned int	i;
-	unsigned int	order;
-	unsigned long	allocated_size;
-	int		ret = 0;
-	struct mc_contg_buffer	*contg_buffer = 0;
-	void		*virt_kernel_addr_stack;
-	void		*phys_addr_stack;
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG("%s (size=%ld)\n", __func__, requested_size);
-
-	order = size_to_order(requested_size);
-	if (order == INVALID_ORDER) {
-		MCDRV_DBG_ERROR(
-			"size to order converting failed for size %ld\n",
-			requested_size);
-		return INVALID_ORDER;
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
 	}
 
-	allocated_size = (1<<order)*PAGE_SIZE;
+	if (ioctl_check_pointer(cmd, uarg))
+		return -EFAULT;
 
-	MCDRV_DBG("size %ld -> order %d --> %ld (2^n pages)\n",
-		  requested_size, order, allocated_size);
-
-	do {
-		/* Usual Wsm request, allocate contigous buffer. */
-		/* search for a free entry in the wsm buffer list
-		 * REV axh: serialize this over multiple instances. */
-		for (i = 0; i < MC_DRV_KMOD_CONTG_BUFFER_MAX; i++) {
-			contg_buffer = &(instance->contg_buffers[i]);
-			if (contg_buffer->handle == 0) {
-				contg_buffer->handle = get_mc_kmod_unique_id();
-				break;
-			}
+	if (ctx.mcp) {
+		while (ctx.mcp->flags.sleep_mode.SleepReq) {
+			ctx.daemon = current;
+			set_current_state(TASK_INTERRUPTIBLE);
+			/* Back off daemon for a while */
+			schedule_timeout(msecs_to_jiffies(DAEMON_BACKOFF_TIME));
+			set_current_state(TASK_RUNNING);
 		}
-		if (i == MC_DRV_KMOD_CONTG_BUFFER_MAX) {
-			MCDRV_DBG_ERROR("no free contigous buffer\n");
-			ret = -EFAULT;
-			break;
-		}
-
-		/* Common code for all allocation paths */
-		virt_kernel_addr_stack = (void *)__get_free_pages(
-							GFP_USER | __GFP_COMP,
-							order);
-		if (virt_kernel_addr_stack == NULL) {
-			MCDRV_DBG_ERROR("get_free_pages failed\n");
-			ret = -ENOMEM;
-			break;
-		}
-
-		/* Get physical address to instance data */
-		phys_addr_stack = (void *)virt_to_phys(virt_kernel_addr_stack);
-		/* TODO: check for INVALID_ADDRESS? */
-
-		MCDRV_DBG(
-			"allocated phys=0x%p - 0x%p, "
-			"size=%ld, kernel_virt=0x%p, handle=%d\n",
-			phys_addr_stack,
-			(void *)((unsigned int)phys_addr_stack+allocated_size),
-			allocated_size,
-			virt_kernel_addr_stack,
-			contg_buffer->handle);
-
-		/* Usual Wsm request, allocate contg_buffer.
-		 *		Also, we never free a persistent Tci */
-		contg_buffer->phys_addr	 = phys_addr_stack;
-		contg_buffer->virt_kernel_addr = virt_kernel_addr_stack;
-		contg_buffer->virt_user_addr   = virt_kernel_addr_stack;
-		contg_buffer->num_pages	 = (1U << order);
-		*handle = contg_buffer->handle;
-		*virt_kernel_addr = virt_kernel_addr_stack;
-		*phys_addr = phys_addr_stack;
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("%s: exit with 0x%08X\n", __func__, ret);
-
-	return ret;
-}
-EXPORT_SYMBOL(mobicore_allocate_wsm);
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * This function will be called from user space as address = mmap(...).
- *
- * @param file
- * @param vmarea
- * vmarea.pg_offset != 0 is mapping of MCI is requested
- *
- * @return 0 if OK or -ENOMEM in case of error.
- */
-static int mc_kernel_module_mmap(
-	struct file		*file,
-	struct vm_area_struct	*vmarea
-)
-{
-	unsigned int		i;
-	unsigned int		order;
-	void			*virt_kernel_addr_stack = 0;
-	void			*phys_addr = 0;
-	unsigned long		requested_size =
-					vmarea->vm_end - vmarea->vm_start;
-	unsigned long		allocated_size;
-	int			ret = 0;
-	struct mc_contg_buffer	*contg_buffer = 0;
-	unsigned int		handle = 0;
-	struct mc_instance	*instance = get_instance(file);
-	unsigned int		request = vmarea->vm_pgoff * 4096;
-#if defined(DEBUG)
-	bool release = false;
-#else
-	bool release = true;
-#endif
-
-	MCDRV_ASSERT(instance != NULL);
-	MCDRV_DBG("enter (vmaStart=0x%p, size=%ld, request=0x%x, mci=0x%x)\n",
-		  (void *)vmarea->vm_start,
-		  requested_size,
-		  request,
-		  mci_base);
-
-	order = size_to_order(requested_size);
-	if (order == INVALID_ORDER) {
-		MCDRV_DBG_ERROR(
-			"size to order converting failed for size %ld\n",
-			requested_size);
-		return -ENOMEM;
 	}
 
-	allocated_size = (1<<order)*PAGE_SIZE;
+	switch (cmd) {
+	case MC_IO_INIT: {
+		struct mc_ioctl_init init;
+		ctx.mcp = NULL;
+		if (!ctx.mci_base.phys) {
+			MCDRV_DBG_ERROR(mcd,
+					"Cannot init MobiCore without MCI!");
+			return -EINVAL;
+		}
+		if (copy_from_user(&init, uarg, sizeof(init)))
+			return -EFAULT;
 
-	MCDRV_DBG("size %ld -> order %d --> %ld (2^n pages)\n",
-		  requested_size, order, allocated_size);
+		ctx.mcp = ctx.mci_base.addr + init.mcp_offset;
+		ret = mc_init((uint32_t)ctx.mci_base.phys, init.nq_offset,
+			init.nq_length, init.mcp_offset, init.mcp_length);
+		break;
+	}
+	case MC_IO_INFO: {
+		struct mc_ioctl_info info;
+		if (copy_from_user(&info, uarg, sizeof(info)))
+			return -EFAULT;
 
-	do {
-		/* Daemon tries to get an existing MCI */
-		if ((request == MC_DRV_KMOD_MMAP_MCI) && (mci_base != 0)) {
-			MCDRV_DBG("Request MCI, it is at (%x)\n", mci_base);
+		ret = mc_info(info.ext_info_id, &info.state,
+			&info.ext_info);
 
-			if (!is_caller_mc_daemon(instance)) {
-				ret = -EPERM;
-				break;
-			}
-			virt_kernel_addr_stack = (void *)mci_base;
-			phys_addr =
-				(void *)virt_to_phys(virt_kernel_addr_stack);
-		} else {
-			/* Usual Wsm request, allocate buffer. */
-			if (request == MC_DRV_KMOD_MMAP_WSM) {
-				/* search for a free entry in the buffer list
-				REV axh: serialize this over multiple instances.
-				*/
-				for (i = 0; i < MC_DRV_KMOD_CONTG_BUFFER_MAX;
-					i++) {
-					contg_buffer =
-						&(instance->contg_buffers[i]);
-					if (contg_buffer->handle == 0) {
-						contg_buffer->handle =
-							get_mc_kmod_unique_id();
-						break;
-					}
-				}
-				if (i == MC_DRV_KMOD_CONTG_BUFFER_MAX) {
-					MCDRV_DBG_ERROR(
-						"no free contigous buffer\n");
-					ret = -EFAULT;
-					break;
-				}
-			} else {
-				if (request <= MC_DRV_KMOD_MMAP_PERSISTENTWSM
-					|| release) {
-					/* Special Wsm request
-						--> only Daemon is allowed */
-					if (!is_caller_mc_daemon(instance)) {
-						ret = -EPERM;
-						break;
-					}
-				}
-			}
-			if (request <= MC_DRV_KMOD_MMAP_PERSISTENTWSM) {
-				/* Common code for all allocation paths
-					*  get physical address, */
-				virt_kernel_addr_stack =
-					(void *)__get_free_pages(
-							GFP_USER | __GFP_COMP,
-							order);
-				if (virt_kernel_addr_stack == NULL) {
-					MCDRV_DBG_ERROR(
-						"get_free_pages failed\n");
-					ret = -ENOMEM;
-					break;
-				}
-				if (request == MC_DRV_KMOD_MMAP_WSM)
-					handle = contg_buffer->handle;
-				/* Get physical address to instance data */
-				/* TODO: check for INVALID_ADDRESS? */
-				phys_addr = (void *)virt_to_phys(
-							virt_kernel_addr_stack);
-			} else {
-#if defined(DEBUG)
-				phys_addr = (void *)request;
-				virt_kernel_addr_stack = phys_to_virt(request);
+		if (!ret) {
+			if (copy_to_user(uarg, &info, sizeof(info)))
+				ret = -EFAULT;
+		}
+		break;
+	}
+	case MC_IO_YIELD:
+		ret = mc_yield();
+		break;
+
+	case MC_IO_NSIQ:
+		ret = mc_nsiq();
+		break;
+
+	case MC_IO_LOCK_WSM: {
+		ret = mc_lock_handle(instance, (uint32_t)arg);
+		break;
+	}
+	case MC_IO_UNLOCK_WSM:
+		ret = mc_unlock_handle(instance, (uint32_t)arg);
+		break;
+	case MC_IO_CLEAN_WSM:
+		ret = mc_clean_wsm_l2(instance);
+		break;
+	case MC_IO_RESOLVE_WSM: {
+		uint32_t handle, phys;
+		if (get_user(handle, uarg))
+			return -EFAULT;
+		phys = mc_find_wsm_l2(instance, handle);
+		if (!phys)
+			return -EFAULT;
+		ret = put_user(phys, uarg);
+		break;
+	}
+	case MC_IO_RESOLVE_CONT_WSM: {
+		struct mc_ioctl_resolv_cont_wsm cont_wsm;
+		uint32_t phys = 0, len = 0;
+		if (copy_from_user(&cont_wsm, uarg, sizeof(cont_wsm)))
+			return -EFAULT;
+		ret = mc_find_cont_wsm(instance, cont_wsm.handle, &phys, &len);
+		if (!ret) {
+			cont_wsm.phys = phys;
+			cont_wsm.length = len;
+			if (copy_to_user(uarg, &cont_wsm, sizeof(cont_wsm)))
+				ret = -EFAULT;
+		}
+		break;
+	}
+	case MC_IO_MAP_MCI:{
+		struct mc_ioctl_map map;
+		if (copy_from_user(&map, uarg, sizeof(map)))
+			return -EFAULT;
+
+		map.reused = (ctx.mci_base.phys != 0);
+		map.phys_addr = (unsigned long)get_mci_base_phys(map.len);
+		if (!map.phys_addr) {
+			MCDRV_DBG_ERROR(mcd, "Failed to setup MCI buffer!");
+			return -EFAULT;
+		}
+
+		if (copy_to_user(uarg, &map, sizeof(map)))
+			ret = -EFAULT;
+		ret = 0;
+		break;
+	}
+	case MC_IO_MAP_PWSM:{
+		break;
+	}
+
+	/* The rest is handled commonly by user IOCTL */
+	default:
+		ret = mc_fd_user_ioctl(file, cmd, arg);
+	} /* end switch(cmd) */
+
+#ifdef MC_MEM_TRACES
+	mobicore_log_read();
 #endif
-			}
-		}
-		/* Common code for all mmap calls:
-		 * map page to user
-		 * store data in page */
-
-		MCDRV_DBG("allocated phys=0x%p - 0x%p, "
-			"size=%ld, kernel_virt=0x%p, handle=%d\n",
-			phys_addr,
-			(void *)((unsigned int)phys_addr+allocated_size),
-			allocated_size, virt_kernel_addr_stack, handle);
-
-		vmarea->vm_flags |= VM_RESERVED;
-		/* convert Kernel address to User Address. Kernel address begins
-			at PAGE_OFFSET, user Address range is below PAGE_OFFSET.
-			Remapping the area is always done, so multiple mappings
-			of one region are possible. Now remap kernel address
-			space into user space */
-		ret = (int)remap_pfn_range(
-				vmarea,
-				(vmarea->vm_start),
-				addr_to_pfn(phys_addr),
-				requested_size,
-				vmarea->vm_page_prot);
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("remapPfnRange failed\n");
-
-			/* free allocated pages when mmap fails, however, do not
-				do it, when daemon tried to get an MCI that
-				existed */
-			if (!((request == MC_DRV_KMOD_MMAP_MCI) &&
-				  (mci_base != 0)))
-				free_continguous_pages(virt_kernel_addr_stack,
-							(1U << order));
-			break;
-		}
-
-		/* Usual Wsm request, allocate contg_buffer.
-			When requesting Mci, we do not associate the page with
-			the process.
-			Note: we also never free the Mci
-			Also, we never free a persistent Tci */
-		if (request == MC_DRV_KMOD_MMAP_WSM) {
-			contg_buffer->phys_addr = phys_addr;
-			contg_buffer->virt_kernel_addr = virt_kernel_addr_stack;
-			contg_buffer->virt_user_addr =
-						(void *)(vmarea->vm_start);
-			contg_buffer->num_pages = (1U << order);
-		}
-
-		/* set response in allocated buffer */
-		{
-			struct mc_mmap_resp *mmap_resp =
-				(struct mc_mmap_resp *)virt_kernel_addr_stack;
-			/* TODO: do this for daemon only, otherwise set NULL */
-			mmap_resp->phys_addr = (uint32_t)phys_addr;
-			mmap_resp->handle = handle;
-			if ((request == MC_DRV_KMOD_MMAP_MCI) &&
-				(mci_base != 0)) {
-				mmap_resp->is_reused = 1;
-			} else
-				mmap_resp->is_reused = 0;
-		}
-
-		/* store MCI pointer */
-		if ((request == MC_DRV_KMOD_MMAP_MCI) && (mci_base == 0)) {
-			mci_base = (uint32_t)virt_kernel_addr_stack;
-			MCDRV_DBG("MCI base set to 0x%x\n", mci_base);
-		}
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
 
 	return (int)ret;
 }
 
-#ifdef CONFIG_SMP
-/*----------------------------------------------------------------------------*/
-/**
- * Force migration of current task to CPU0(where the monitor resides)
+/*
+ * mc_fd_read() - This will be called from user space as read(...)
+ * @file:	file pointer
+ * @buffer:	buffer where to copy to(userspace)
+ * @buffer_len:	number of requested data
+ * @pos:	not used
  *
- * @return Error code or 0 for success
- */
-static int goto_cpu0(
-	void
-)
-{
-	int		ret = 0;
-	struct cpumask	mask =  CPU_MASK_CPU0;
-
-	MCDRV_DBG_VERBOSE("System has %d CPU's, we are on CPU #%d\n"
-		  "\tBinding this process to CPU #0.\n"
-		  "\tactive mask is %lx, setting it to mask=%lx\n",
-		  nr_cpu_ids,
-		  raw_smp_processor_id(),
-		  cpu_active_mask->bits[0],
-		  mask.bits[0]);
-	ret = set_cpus_allowed_ptr(current, &mask);
-	if (ret != 0)
-		MCDRV_DBG_ERROR("set_cpus_allowed_ptr=%d.\n", ret);
-	MCDRV_DBG_VERBOSE("And now we are on CPU #%d\n",
-				raw_smp_processor_id());
-
-	return ret;
-}
-
-/*----------------------------------------------------------------------------*/
-/**
- * Restore CPU mask for current to ALL Cpus(reverse of goto_cpu0)
+ * The read function is blocking until a interrupt occurs. In that case the
+ * event counter is copied into user space and the function is finished.
  *
- * @return Error code or 0 for success
+ * If OK this function returns the number of copied data otherwise it returns
+ * errno
  */
-static int goto_all_cpu(
-	void
-)
+static ssize_t mc_fd_read(struct file *file, char *buffer, size_t buffer_len,
+			  loff_t *pos)
 {
-	int		ret = 0;
+	int ret = 0, ssiq_counter;
+	struct mc_instance *instance = get_instance(file);
 
-	struct cpumask	mask =  CPU_MASK_ALL;
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
 
-	MCDRV_DBG_VERBOSE("System has %d CPU's, we are on CPU #%d\n"
-		  "\tBinding this process to CPU #0.\n"
-		  "\tactive mask is %lx, setting it to mask=%lx\n",
-		  nr_cpu_ids,
-		  raw_smp_processor_id(),
-		  cpu_active_mask->bits[0],
-		  mask.bits[0]);
-	ret = set_cpus_allowed_ptr(current, &mask);
-	if (ret != 0)
-		MCDRV_DBG_ERROR("set_cpus_allowed_ptr=%d.\n", ret);
-	MCDRV_DBG_VERBOSE("And now we are on CPU #%d\n",
-				raw_smp_processor_id());
+	/* avoid debug output on non-error, because this is call quite often */
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
 
-	return ret;
+	/* only the MobiCore Daemon is allowed to call this function */
+	if (WARN_ON(!is_daemon(instance))) {
+		MCDRV_DBG_ERROR(mcd, "caller not MobiCore Daemon\n");
+		return -EPERM;
+	}
+
+	if (buffer_len < sizeof(unsigned int)) {
+		MCDRV_DBG_ERROR(mcd, "invalid length\n");
+		return -EINVAL;
+	}
+
+	for (;;) {
+		if (wait_for_completion_interruptible(&ctx.isr_comp)) {
+			MCDRV_DBG_VERBOSE(mcd, "read interrupted\n");
+			return -ERESTARTSYS;
+		}
+
+		ssiq_counter = atomic_read(&ctx.isr_counter);
+		MCDRV_DBG_VERBOSE(mcd, "ssiq_counter=%i, ctx.counter=%i\n",
+				  ssiq_counter, ctx.evt_counter);
+
+		if (ssiq_counter != ctx.evt_counter) {
+			/* read data and exit loop without error */
+			ctx.evt_counter = ssiq_counter;
+			ret = 0;
+			break;
+		}
+
+		/* end loop if non-blocking */
+		if (file->f_flags & O_NONBLOCK) {
+			MCDRV_DBG_ERROR(mcd, "non-blocking read\n");
+			return -EAGAIN;
+		}
+
+		if (signal_pending(current)) {
+			MCDRV_DBG_VERBOSE(mcd, "received signal.\n");
+			return -ERESTARTSYS;
+		}
+	}
+
+	/* read data and exit loop */
+	ret = copy_to_user(buffer, &ctx.evt_counter, sizeof(unsigned int));
+
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "copy_to_user failed\n");
+		return -EFAULT;
+	}
+
+	ret = sizeof(unsigned int);
+
+	return (ssize_t)ret;
 }
 
-#else
-static int goto_cpu0(void)
-{
-	return 0;
-}
-
-static int goto_all_cpu(void)
-{
-	return 0;
-}
-#endif
-
-/*----------------------------------------------------------------------------*/
-/**
+/*
  * Initialize a new mobicore API instance object
  *
  * @return Instance or NULL if no allocation was possible.
  */
-struct mc_instance *mobicore_open(
-	void
-) {
-	struct mc_instance	*instance;
-	pid_t			pid_vnr;
+struct mc_instance *mc_alloc_instance(void)
+{
+	struct mc_instance *instance;
 
 	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
 	if (instance == NULL)
 		return NULL;
 
 	/* get a unique ID for this instance (PIDs are not unique) */
-	instance->handle = get_mc_kmod_unique_id();
+	instance->handle = get_unique_id();
 
-	/* get the PID of the calling process. We avoid using
-	 *	current->pid directly, as 2.6.24 introduced PID
-	 *	namespaces. See also http://lwn.net/Articles/259217 */
-	pid_vnr = task_pid_vnr(current);
-	instance->pid_vnr = pid_vnr;
+	mutex_init(&instance->lock);
 
 	return instance;
 }
-EXPORT_SYMBOL(mobicore_open);
 
-/*----------------------------------------------------------------------------*/
-/**
- * This function will be called from user space as fd = open(...).
+/*
+ * Release a mobicore instance object and all objects related to it
+ * @instance:	instance
+ * Returns 0 if Ok or -E ERROR
+ */
+int mc_release_instance(struct mc_instance *instance)
+{
+	struct mc_buffer *buffer, *tmp;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	mutex_lock(&instance->lock);
+	mc_clear_l2_tables(instance);
+
+	mutex_lock(&ctx.bufs_lock);
+	/* release all mapped data */
+
+	/* Check if some buffers are orphaned. */
+	list_for_each_entry_safe(buffer, tmp, &ctx.cont_bufs, list) {
+		if (buffer->instance == instance) {
+			buffer->instance = NULL;
+			free_buffer(buffer);
+		}
+	}
+	mutex_unlock(&ctx.bufs_lock);
+
+	mutex_unlock(&instance->lock);
+
+	/* release instance context */
+	kfree(instance);
+
+	return 0;
+}
+
+/*
+ * mc_fd_user_open() - Will be called from user space as fd = open(...)
  * A set of internal instance data are created and initialized.
  *
- * @param inode
- * @param file
- * @return 0 if OK or -ENOMEM if no allocation was possible.
+ * @inode
+ * @file
+ * Returns 0 if OK or -ENOMEM if no allocation was possible.
  */
-static int mc_kernel_module_open(
-	struct inode	*inode,
-	struct file	*file
-)
+static int mc_fd_user_open(struct inode *inode, struct file *file)
 {
-	struct mc_instance	*instance;
-	int			ret = 0;
+	struct mc_instance *instance;
 
-	MCDRV_DBG_VERBOSE("enter\n");
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
 
-	do {
-		instance = mobicore_open();
-		if (instance == NULL)
-			return -ENOMEM;
+	instance = mc_alloc_instance();
+	if (instance == NULL)
+		return -ENOMEM;
 
-		/* check if Daemon. We simply assume that the first to open us
-			with root privileges must be the daemon. */
-		if ((is_userland_caller_privileged())
-			&& (mc_drv_kmod_ctx.daemon_inst == NULL)) {
-			MCDRV_DBG("accept this as MobiCore Daemon\n");
+	/* store instance data reference */
+	file->private_data = instance;
 
-			/* Set the caller's CPU mask to CPU0*/
-			ret = goto_cpu0();
-			if (ret != 0) {
-				mobicore_release(instance);
-				file->private_data = NULL;
-				MCDRV_DBG("changing core failed!\n");
-				break;
-			}
-
-			mc_drv_kmod_ctx.daemon_inst = instance;
-			sema_init(&mc_drv_kmod_ctx.daemon_ctx.sem,
-					DAEMON_SEM_VAL);
-			/* init ssiq event counter */
-			mc_drv_kmod_ctx.daemon_ctx.ssiq_counter =
-				atomic_read(
-					&(mc_drv_kmod_ctx.ssiq_ctx.counter));
-
-#ifdef MC_MEM_TRACES
-			/* The traces have to be setup on CPU-0 since we must
-			 * do a fastcall to MobiCore. */
-			if (!mci_base)
-				/* Do the work only if MCI base is not
-				 * initialized properly */
-				work_on_cpu(0, mobicore_log_setup, NULL);
-#endif
-		}
-
-		/* store instance data reference */
-		file->private_data = instance;
-
-		/* TODO axh: link all instances to allow clean up? */
-
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
-
-	return (int)ret;
-
+	return 0;
 }
 
-/*----------------------------------------------------------------------------*/
-/**
- * Release a mobicore instance object and all objects related to it
- * @param instance instance
- * @return 0 if Ok or -E ERROR
- */
-int mobicore_release(
-	struct mc_instance	*instance
-)
+static int mc_fd_admin_open(struct inode *inode, struct file *file)
 {
-	int ret = 0;
-	int i;
-	struct mc_used_l2_table	*used_l2table, *used_l2table_temp;
+	struct mc_instance *instance;
 
-	do {
-		/* try to get the semaphore */
-		ret = down_interruptible(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		if (ret != 0) {
-			MCDRV_DBG_ERROR(
-				"down_interruptible() failed with %d\n", ret);
-			/* TODO: can be block here? */
-			ret = -ERESTARTSYS;
-		} else {
-			/* Check if some WSM is still in use. */
-			list_for_each_entry_safe(
-				used_l2table,
-				used_l2table_temp,
-				&(mc_drv_kmod_ctx.mc_used_l2_tables),
-				list
-			) {
-				if (used_l2table->owner == instance) {
-					MCDRV_DBG_WARN(
-						"trying to release WSM L2: "
-						"physBase=%p ,nr_of_pages=%d\n",
-						get_l2_table_phys(used_l2table),
-						used_l2table->nr_of_pages);
+	/*
+	 * The daemon is already set so we can't allow anybody else to open
+	 * the admin interface.
+	 */
+	if (ctx.daemon_inst) {
+		MCDRV_DBG_ERROR(mcd, "Daemon is already connected");
+		return -EPERM;
+	}
+	/* Setup the usual variables */
+	if (mc_fd_user_open(inode, file))
+		return -ENOMEM;
+	instance = get_instance(file);
 
-					/* unlock app usage and free if MobiCore
-					does not use it */
-					delete_used_l2_table(used_l2table,
-							FREE_FROM_NWD);
-				}
-			} /* end while */
+	MCDRV_DBG(mcd, "accept this as MobiCore Daemon\n");
 
-			/* release semaphore */
-			up(&(mc_drv_kmod_ctx.wsm_l2_sem));
-		}
+	ctx.daemon_inst = instance;
+	ctx.daemon = current;
+	instance->admin = true;
+	init_completion(&ctx.isr_comp);
+	/* init ssiq event counter */
+	ctx.evt_counter = atomic_read(&(ctx.isr_counter));
 
-
-		/* release all mapped data */
-		for (i = 0; i < MC_DRV_KMOD_CONTG_BUFFER_MAX; i++) {
-			struct mc_contg_buffer *contg_buffer =
-					&(instance->contg_buffers[i]);
-
-			if (contg_buffer->virt_user_addr != 0) {
-				free_continguous_pages(
-					contg_buffer->virt_kernel_addr,
-					contg_buffer->num_pages);
-			}
-		}
-
-		/* release instance context */
-		kfree(instance);
-	} while (FALSE);
-
-	return ret;
+	return 0;
 }
-EXPORT_SYMBOL(mobicore_release);
 
-/*----------------------------------------------------------------------------*/
-/**
- * This function will be called from user space as close(...).
+/*
+ * mc_fd_release() - This function will be called from user space as close(...)
  * The instance data are freed and the associated memory pages are unreserved.
  *
- * @param inode
- * @param file
+ * @inode
+ * @file
  *
- * @return 0
+ * Returns 0
  */
-static int mc_kernel_module_release(
-	struct inode	*inode,
-	struct file	*file
-)
+static int mc_fd_release(struct inode *inode, struct file *file)
 {
-	int			ret = 0;
-	struct mc_instance	*instance = get_instance(file);
+	int ret = 0;
+	struct mc_instance *instance = get_instance(file);
 
-	MCDRV_DBG_VERBOSE("enter\n");
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
 
-	do {
-		/* check if daemon closes us. */
-		if (is_caller_mc_daemon(instance)) {
-			/* TODO: cleanup?
-				* mc_drv_kmod_ctx.mc_used_l2_tables remains */
-			MCDRV_DBG_WARN("WARNING: MobiCore Daemon died\n");
-			mc_drv_kmod_ctx.daemon_inst = NULL;
-		}
+	/* check if daemon closes us. */
+	if (is_daemon(instance)) {
+		MCDRV_DBG_WARN(mcd, "WARNING: MobiCore Daemon died\n");
+		ctx.daemon_inst = NULL;
+		ctx.daemon = NULL;
+	}
 
-		ret = mobicore_release(instance);
+	ret = mc_release_instance(instance);
 
-	} while (FALSE);
-
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
+	/*
+	 * ret is quite irrelevant here as most apps don't care about the
+	 * return value from close() and it's quite difficult to recover
+	 */
+	MCDRV_DBG_VERBOSE(mcd, "exit with %d/0x%08X\n", ret, ret);
 
 	return (int)ret;
 }
 
-
-/*----------------------------------------------------------------------------*/
-/**
+/*
  * This function represents the interrupt function of the mcDrvModule.
  * It signals by incrementing of an event counter and the start of the read
  * waiting queue, the read function a interrupt has occurred.
- *
- * @param   intr
- * @param   *context  pointer to registered device data
- *
- * @return  IRQ_HANDLED
  */
-static irqreturn_t mc_kernel_module_intr_ssiq(
-	int	intr,
-	void	*context
-)
+static irqreturn_t mc_ssiq_isr(int intr, void *context)
 {
-	irqreturn_t	ret = IRQ_NONE;
+	/* increment interrupt event counter */
+	atomic_inc(&(ctx.isr_counter));
 
-	/* we know the context. */
-	MCDRV_ASSERT(&mc_drv_kmod_ctx == context);
+	/* signal the daemon */
+	complete(&ctx.isr_comp);
 
-	do {
-		if (intr != MC_INTR_SSIQ) {
-			/* this should not happen, as we did no register for any
-				other interrupt. For debugging, we print a
-				message, but continue */
-			MCDRV_DBG_WARN(
-				"unknown interrupt %d, expecting only %d\n",
-				intr, MC_INTR_SSIQ);
-		}
-		MCDRV_DBG_VERBOSE("received interrupt %d\n",
-				  intr);
-
-		/* increment interrupt event counter */
-		atomic_inc(&(mc_drv_kmod_ctx.ssiq_ctx.counter));
-
-		/* signal the daemon */
-		up(&mc_drv_kmod_ctx.daemon_ctx.sem);
-
-
-		ret = IRQ_HANDLED;
-
-	} while (FALSE);
-
-	return ret;
+	return IRQ_HANDLED;
 }
 
-/*----------------------------------------------------------------------------*/
-/** function table structure of this device driver. */
-static const struct file_operations mc_kernel_module_file_operations = {
-	.owner		= THIS_MODULE, /**< driver owner */
-	.open		= mc_kernel_module_open, /**< driver open function */
-	.release	= mc_kernel_module_release, /**< driver release function*/
-	.unlocked_ioctl	= mc_kernel_module_ioctl, /**< driver ioctl function */
-	.mmap		= mc_kernel_module_mmap, /**< driver mmap function */
-	.read		= mc_kernel_module_read, /**< driver read function */
+/* function table structure of this device driver. */
+static const struct file_operations mc_admin_fops = {
+	.owner		= THIS_MODULE,
+	.open		= mc_fd_admin_open,
+	.release	= mc_fd_release,
+	.unlocked_ioctl	= mc_fd_admin_ioctl,
+	.mmap		= mc_fd_mmap,
+	.read		= mc_fd_read,
 };
 
-/*----------------------------------------------------------------------------*/
-/** registration structure as miscdevice. */
-static struct miscdevice mc_kernel_module_device = {
-	.name	= MC_DRV_MOD_DEVNODE, /**< device name */
-	.minor	= MISC_DYNAMIC_MINOR, /**< device minor number */
-	/** device interface function structure */
-	.fops	= &mc_kernel_module_file_operations,
+static struct miscdevice mc_admin_device = {
+	.name	= MC_ADMIN_DEVNODE,
+	.mode	= (S_IRWXU),
+	.minor	= MISC_DYNAMIC_MINOR,
+	.fops	= &mc_admin_fops,
 };
 
+/* function table structure of this device driver. */
+static const struct file_operations mc_user_fops = {
+	.owner		= THIS_MODULE,
+	.open		= mc_fd_user_open,
+	.release	= mc_fd_release,
+	.unlocked_ioctl	= mc_fd_user_ioctl,
+	.mmap		= mc_fd_mmap,
+};
 
-/*----------------------------------------------------------------------------*/
-/**
+static struct miscdevice mc_user_device = {
+	.name	= MC_USER_DEVNODE,
+	.mode	= (S_IRWXU | S_IRWXG | S_IRWXO),
+	.minor	= MISC_DYNAMIC_MINOR,
+	.fops	= &mc_user_fops,
+};
+
+/*
  * This function is called the kernel during startup or by a insmod command.
  * This device is installed and registered as miscdevice, then interrupt and
  * queue handling is set up
- *
- * @return 0 for no error or -EIO if registration fails
  */
-static int __init mc_kernel_module_init(
-	void
-)
+static int __init mobicore_init(void)
 {
 	int ret = 0;
 
-	MCDRV_DBG("enter (Build " __TIMESTAMP__ ")\n");
-	MCDRV_DBG("mcDrvModuleApi version is %i.%i\n",
-			MCDRVMODULEAPI_VERSION_MAJOR,
-			MCDRVMODULEAPI_VERSION_MINOR);
+	dev_set_name(mcd, "mcd");
+
+	MCDRV_DBG(mcd, "enter (Build " __TIMESTAMP__ ")\n");
+	MCDRV_DBG(mcd, "mcDrvModuleApi version is %i.%i\n",
+		  MCDRVMODULEAPI_VERSION_MAJOR,
+		  MCDRVMODULEAPI_VERSION_MINOR);
 #ifdef MOBICORE_COMPONENT_BUILD_TAG
-	MCDRV_DBG("%s\n", MOBICORE_COMPONENT_BUILD_TAG);
+	MCDRV_DBG(mcd, "%s\n", MOBICORE_COMPONENT_BUILD_TAG);
 #endif
-	do {
-		/* Hardware does not support ARM TrustZone
-			-> Cannot continue! */
-		if (!has_security_extensions()) {
-			MCDRV_DBG_ERROR(
-				"Hardware does't support ARM TrustZone!\n");
-			ret = -ENODEV;
-			break;
-		}
+	/* Hardware does not support ARM TrustZone -> Cannot continue! */
+	if (!has_security_extensions()) {
+		MCDRV_DBG_ERROR(mcd,
+				"Hardware doesn't support ARM TrustZone!\n");
+		return -ENODEV;
+	}
 
-		/* Running in secure mode -> Cannot load the driver! */
-		if (is_secure_mode()) {
-			MCDRV_DBG_ERROR("Running in secure MODE!\n");
-			ret = -ENODEV;
-			break;
-		}
+	/* Running in secure mode -> Cannot load the driver! */
+	if (is_secure_mode()) {
+		MCDRV_DBG_ERROR(mcd, "Running in secure MODE!\n");
+		return -ENODEV;
+	}
 
-		sema_init(&mc_drv_kmod_ctx.daemon_ctx.sem, DAEMON_SEM_VAL);
-		/* set up S-SIQ interrupt handler */
-		ret = request_irq(
-				  MC_INTR_SSIQ,
-				  mc_kernel_module_intr_ssiq,
-				  IRQF_TRIGGER_RISING,
-				  MC_DRV_MOD_DEVNODE,
-				  &mc_drv_kmod_ctx);
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("interrupt request failed\n");
-			break;
-		}
+	init_completion(&ctx.isr_comp);
+	/* set up S-SIQ interrupt handler */
+	ret = request_irq(MC_INTR_SSIQ, mc_ssiq_isr, IRQF_TRIGGER_RISING,
+			MC_ADMIN_DEVNODE, &ctx);
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "interrupt request failed\n");
+		goto error;
+	}
 
-		ret = misc_register(&mc_kernel_module_device);
-		if (ret != 0) {
-			MCDRV_DBG_ERROR("device register failed\n");
-			break;
-		}
+#ifdef MC_PM_RUNTIME
+	ret = mc_pm_initialize(&ctx);
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "Power Management init failed!\n");
+		goto free_isr;
+	}
+#endif
 
-		/* initialize event counter for signaling of an IRQ to zero */
-		atomic_set(&(mc_drv_kmod_ctx.ssiq_ctx.counter), 0);
+	ret = misc_register(&mc_admin_device);
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "admin device register failed\n");
+		goto free_isr;
+	}
 
-		/* init list for WSM L2 chunks. */
-		INIT_LIST_HEAD(&(mc_drv_kmod_ctx.mc_l2_tables_sets));
+	ret = misc_register(&mc_user_device);
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "user device register failed\n");
+		goto free_admin;
+	}
 
-		/* L2 table descriptor list. */
-		INIT_LIST_HEAD(&(mc_drv_kmod_ctx.mc_used_l2_tables));
+#ifdef MC_MEM_TRACES
+	mobicore_log_setup();
+#endif
 
-		sema_init(&(mc_drv_kmod_ctx.wsm_l2_sem), 1);
+	/* initialize event counter for signaling of an IRQ to zero */
+	atomic_set(&ctx.isr_counter, 0);
 
-		/* initialize unique number counter which we can use for
-			handles. It is limited to 2^32, but this should be
-			enough to be roll-over safe for us. We start with 1
-			instead of 0. */
-		atomic_set(&(mc_drv_kmod_ctx.unique_counter), 1);
+	ret = mc_init_l2_tables();
 
-		mci_base = 0;
-		MCDRV_DBG("initialized\n");
+	/*
+	 * initialize unique number counter which we can use for
+	 * handles. It is limited to 2^32, but this should be
+	 * enough to be roll-over safe for us. We start with 1
+	 * instead of 0.
+	 */
+	atomic_set(&ctx.unique_counter, 1);
 
-		ret = 0;
+	/* init list for contiguous buffers  */
+	INIT_LIST_HEAD(&ctx.cont_bufs);
 
-	} while (FALSE);
+	/* init lock for the buffers list */
+	mutex_init(&ctx.bufs_lock);
 
-	MCDRV_DBG_VERBOSE("exit with %d/0x%08X\n", ret, ret);
+	memset(&ctx.mci_base, 0, sizeof(ctx.mci_base));
+	MCDRV_DBG(mcd, "initialized\n");
+	return 0;
 
-	return (int)ret;
+free_admin:
+	misc_deregister(&mc_admin_device);
+free_isr:
+	free_irq(MC_INTR_SSIQ, &ctx);
+error:
+	return ret;
 }
 
-
-
-/*----------------------------------------------------------------------------*/
-/**
+/*
  * This function removes this device driver from the Linux device manager .
  */
-static void __exit mc_kernel_module_exit(
-	void
-)
+static void __exit mobicore_exit(void)
 {
-	struct mc_used_l2_table	*used_l2table;
-
-	MCDRV_DBG_VERBOSE("enter\n");
-
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
+#ifdef MC_MEM_TRACES
 	mobicore_log_free();
+#endif
 
-	/* Check if some WSM is still in use. */
-	list_for_each_entry(
-		used_l2table,
-		&(mc_drv_kmod_ctx.mc_used_l2_tables),
-		list
-	) {
-		MCDRV_DBG_WARN(
-			"WSM L2 still in use: physBase=%p ,nr_of_pages=%d\n",
-			get_l2_table_phys(used_l2table),
-			used_l2table->nr_of_pages);
-	} /* end while */
+	mc_release_l2_tables();
 
-	free_irq(MC_INTR_SSIQ, &mc_drv_kmod_ctx);
+#ifdef MC_PM_RUNTIME
+	mc_pm_free();
+#endif
 
-	misc_deregister(&mc_kernel_module_device);
-	MCDRV_DBG_VERBOSE("exit");
+	free_irq(MC_INTR_SSIQ, &ctx);
+
+	misc_deregister(&mc_admin_device);
+	misc_deregister(&mc_user_device);
+	MCDRV_DBG_VERBOSE(mcd, "exit");
 }
 
-
-/*----------------------------------------------------------------------------*/
 /* Linux Driver Module Macros */
-module_init(mc_kernel_module_init);
-module_exit(mc_kernel_module_exit);
+module_init(mobicore_init);
+module_exit(mobicore_exit);
 MODULE_AUTHOR("Giesecke & Devrient GmbH");
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("MobiCore driver");
 
-/** @} */
-
diff --git a/drivers/gud/mobicore_driver/main.h b/drivers/gud/mobicore_driver/main.h
new file mode 100644
index 0000000..e23c516
--- /dev/null
+++ b/drivers/gud/mobicore_driver/main.h
@@ -0,0 +1,144 @@
+/*
+ * Header file of MobiCore Driver Kernel Module.
+ *
+ * Internal structures of the McDrvModule
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_MAIN_H_
+#define _MC_MAIN_H_
+
+#include <asm/pgtable.h>
+#include <linux/semaphore.h>
+#include <linux/completion.h>
+#include <linux/mutex.h>
+
+#include "public/mc_linux.h"
+/* Platform specific settings */
+#include "platform.h"
+
+#define MC_VERSION(major, minor) \
+		(((major & 0x0000ffff) << 16) | (minor & 0x0000ffff))
+
+/* Instance data for MobiCore Daemon and TLCs. */
+struct mc_instance {
+	/* Instance lock */
+	struct mutex lock;
+	/* unique handle */
+	unsigned int handle;
+	bool admin;
+};
+
+/*
+ * Contiguous buffer allocated to TLCs.
+ * These buffers are uses as world shared memory (wsm) and shared with
+ * secure world.
+ * The virtual kernel address is added for a simpler search algorithm.
+ */
+struct mc_buffer {
+	struct list_head	list;
+	/* unique handle */
+	unsigned int		handle;
+	/* Number of references kept to this buffer */
+	atomic_t		usage;
+	/* virtual Kernel start address */
+	void			*addr;
+	/* physical start address */
+	void			*phys;
+	/* order of number of pages */
+	unsigned int		order;
+	uint32_t		len;
+	struct mc_instance	*instance;
+};
+
+/* MobiCore Driver Kernel Module context data. */
+struct mc_context {
+	/* MobiCore MCI information */
+	struct mc_buffer	mci_base;
+	/* MobiCore MCP buffer */
+	struct mc_mcp_buffer	*mcp;
+	/* event completion */
+	struct completion	isr_comp;
+	/* isr event counter */
+	unsigned int		evt_counter;
+	atomic_t		isr_counter;
+	/* ever incrementing counter */
+	atomic_t		unique_counter;
+	/* pointer to instance of daemon */
+	struct mc_instance	*daemon_inst;
+	/* pointer to instance of daemon */
+	struct task_struct	*daemon;
+	/* General list of contiguous buffers allocated by the kernel */
+	struct list_head	cont_bufs;
+	/* Lock for the list of contiguous buffers */
+	struct mutex		bufs_lock;
+};
+
+struct mc_sleep_mode {
+	uint16_t	SleepReq;
+	uint16_t	ReadyToSleep;
+};
+
+/* MobiCore is idle. No scheduling required. */
+#define SCHEDULE_IDLE		0
+/* MobiCore is non idle, scheduling is required. */
+#define SCHEDULE_NON_IDLE	1
+
+/* MobiCore status flags */
+struct mc_flags {
+	/*
+	 * Scheduling hint: if <> SCHEDULE_IDLE, MobiCore should
+	 * be scheduled by the NWd
+	 */
+	uint32_t		schedule;
+	/* State of sleep protocol */
+	struct mc_sleep_mode	sleep_mode;
+	/* Reserved for future use: Must not be interpreted */
+	uint32_t		rfu[2];
+};
+
+/* MCP buffer structure */
+struct mc_mcp_buffer {
+	/* MobiCore Flags */
+	struct mc_flags	flags;
+	uint32_t	rfu; /* MCP message buffer - ignore */
+};
+
+unsigned int get_unique_id(void);
+
+/* check if caller is MobiCore Daemon */
+static inline bool is_daemon(struct mc_instance *instance)
+{
+	if (!instance)
+		return false;
+	return instance->admin;
+}
+
+
+/* Initialize a new mobicore API instance object */
+struct mc_instance *mc_alloc_instance(void);
+/* Release a mobicore instance object and all objects related to it */
+int mc_release_instance(struct mc_instance *instance);
+
+/*
+ * mc_register_wsm_l2() - Create a L2 table from a virtual memory buffer which
+ * can be vmalloc or user space virtual memory
+ */
+int mc_register_wsm_l2(struct mc_instance *instance,
+	uint32_t buffer, uint32_t len,
+	uint32_t *handle, uint32_t *phys);
+/* Unregister the buffer mapped above */
+int mc_unregister_wsm_l2(struct mc_instance *instance, uint32_t handle);
+
+/* Allocate one mc_buffer of contiguous space */
+int mc_get_buffer(struct mc_instance *instance,
+	struct mc_buffer **buffer, unsigned long len);
+/* Free the buffer allocated above */
+int mc_free_buffer(struct mc_instance *instance, uint32_t handle);
+
+#endif /* _MC_MAIN_H_ */
diff --git a/drivers/gud/mobicore_driver/mc_drv_module.h b/drivers/gud/mobicore_driver/mc_drv_module.h
deleted file mode 100644
index 8b402d6..0000000
--- a/drivers/gud/mobicore_driver/mc_drv_module.h
+++ /dev/null
@@ -1,238 +0,0 @@
-/**
- * Header file of MobiCore Driver Kernel Module.
- *
- * @addtogroup MCD_MCDIMPL_KMOD_IMPL
- * @{
- * Internal structures of the McDrvModule
- * @file
- *
- * Header file the MobiCore Driver Kernel Module,
- * its internal structures and defines.
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef _MC_DRV_KMOD_H_
-#define _MC_DRV_KMOD_H_
-
-#include "mc_drv_module_linux_api.h"
-#include "public/mc_drv_module_api.h"
-/** Platform specific settings */
-#include "platform.h"
-
-/** ARM Specific masks and modes */
-#define ARM_CPSR_MASK 0x1F
-#define ARM_MONITOR_MODE 0b10110
-#define ARM_SECURITY_EXTENSION_MASK 0x30
-
-/**
- * Number of page table entries in one L2 table. This is ARM specific, an
- *  L2 table covers 1 MiB by using 256 entry referring to 4KiB pages each.
- */
-#define MC_ARM_L2_TABLE_ENTRIES		256
-
-/** Maximum number of contiguous buffer allocations for one driver instance. */
-#define MC_DRV_KMOD_CONTG_BUFFER_MAX	16
-
-/** Number of pages for L2 tables. There are 4 table in each page. */
-#define MC_DRV_KMOD_L2_TABLE_PER_PAGES	4
-
-/** ARM level 2 (L2) table with 256 entries. Size: 1k */
-struct l2table {
-	pte_t	table_entries[MC_ARM_L2_TABLE_ENTRIES];
-};
-
-#define INVALID_ADDRESS     ((void *)(-1))
-
-/** ARM L2 PTE bits */
-#define L2_FLAG_SMALL_XN    (1U <<  0)
-#define L2_FLAG_SMALL       (1U <<  1)
-#define L2_FLAG_B           (1U <<  2)
-#define L2_FLAG_C           (1U <<  3)
-#define L2_FLAG_AP0         (1U <<  4)
-#define L2_FLAG_AP1         (1U <<  5)
-#define L2_FLAG_SMALL_TEX0  (1U <<  6)
-#define L2_FLAG_SMALL_TEX1  (1U <<  7)
-#define L2_FLAG_SMALL_TEX2  (1U <<  8)
-#define L2_FLAG_APX         (1U <<  9)
-#define L2_FLAG_S           (1U << 10)
-#define L2_FLAG_NG          (1U << 11)
-
-/**
- * Contiguous buffer allocated to TLCs.
- * These buffers are uses as world shared memory (wsm) and shared with
- * secure world.
- * The virtual kernel address is added for a simpler search algorithm.
- */
-struct mc_contg_buffer {
-	unsigned int	handle; /* unique handle */
-	void		*virt_user_addr; /**< virtual User start address */
-	void		*virt_kernel_addr; /**< virtual Kernel start address */
-	void		*phys_addr; /**< physical start address */
-	unsigned int	num_pages; /**< number of pages */
-};
-
-/** Instance data for MobiCore Daemon and TLCs. */
-struct mc_instance {
-	/** unique handle */
-	unsigned int	handle;
-	/** process that opened this instance */
-	pid_t		pid_vnr;
-	/** buffer list for mmap generated address space and
-		its virtual client address */
-	struct mc_contg_buffer	contg_buffers[MC_DRV_KMOD_CONTG_BUFFER_MAX];
-};
-
-/** Store for four L2 tables in one 4kb page*/
-struct mc_l2_table_store {
-	struct l2table table[MC_DRV_KMOD_L2_TABLE_PER_PAGES];
-};
-
-/** Usage and maintenance information about mc_l2_table_store */
-struct mc_l2_tables_set {
-	struct list_head		list;
-	unsigned int			usage_bitmap;	/**< usage bitmap */
-	struct mc_l2_table_store	*kernel_virt;	/**< kernel virtual address */
-	struct mc_l2_table_store	*phys;		/**< physical address */
-	struct page			*page;		/**< pointer to page struct */
-};
-
-/**
- * L2 table allocated to the Daemon or a TLC describing a world shared buffer.
- * When users map a malloc()ed area into SWd, a L2 table is allocated.
- * In addition, the area of maximum 1MB virtual address space is mapped into
- * the L2 table and a handle for this table is returned to the user.
- */
-struct mc_used_l2_table {
-	struct list_head	list;
-
-	/** handle as communicated to user mode */
-	unsigned int		handle;
-	unsigned int		flags;
-
-	/** owner of this L2 table */
-	struct mc_instance	*owner;
-
-	/** set describing where our L2 table is stored */
-	struct mc_l2_tables_set	*set;
-
-	/** index into L2 table set */
-	unsigned int		idx;
-
-	/** size of buffer */
-	unsigned int		nr_of_pages;
-};
-
-#define MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_APP   (1U << 0)
-#define MC_WSM_L2_CONTAINER_WSM_LOCKED_BY_MC    (1U << 1)
-
-
-/** MobiCore S-SIQ interrupt context data. */
-struct mc_ssiq_ctx {
-	/** S-SIQ interrupt counter */
-	atomic_t	counter;
-};
-
-/** MobiCore Daemon context data. */
-struct mc_daemon_ctx {
-	/** event semaphore */
-	struct semaphore	sem;
-	struct fasync_struct	*async_queue;
-	/** event counter */
-	unsigned int		ssiq_counter;
-};
-
-/** MobiCore Driver Kernel Module context data. */
-struct mc_drv_kmod_ctx {
-
-	/** ever incrementing counter */
-	atomic_t		unique_counter;
-
-	/** S-SIQ interrupt context */
-	struct mc_ssiq_ctx	ssiq_ctx;
-
-	/** MobiCore Daemon context */
-	struct mc_daemon_ctx	daemon_ctx;
-
-	/** pointer to instance of daemon */
-	struct mc_instance	*daemon_inst;
-
-	/** Backing store for L2 tables */
-	struct list_head	mc_l2_tables_sets;
-
-	/** Bookkeeping for used L2 tables */
-	struct list_head	mc_used_l2_tables;
-
-	/** semaphore to synchronize access to above lists */
-	struct semaphore	wsm_l2_sem;
-};
-
-/** MobiCore internal trace buffer structure. */
-struct mc_trace_buf {
-	uint32_t version; /**< version of trace buffer */
-	uint32_t length; /**< length of allocated buffer(includes header) */
-	uint32_t write_pos; /**< last write position */
-	char  buff[1]; /**< start of the log buffer */
-};
-
-/*** MobiCore internal trace log setup. */
-void mobicore_log_read(void);
-long mobicore_log_setup(void *);
-void mobicore_log_free(void);
-
-#define MCDRV_DBG_ERROR(txt, ...) \
-	printk(KERN_ERR "mcDrvKMod [%d] %s() ### ERROR: " txt, \
-		task_pid_vnr(current), \
-		__func__, \
-		##__VA_ARGS__)
-
-/* dummy function helper macro. */
-#define DUMMY_FUNCTION()    do {} while (0)
-
-#if defined(DEBUG)
-
-/* #define DEBUG_VERBOSE */
-#if defined(DEBUG_VERBOSE)
-#define MCDRV_DBG_VERBOSE          MCDRV_DBG
-#else
-#define MCDRV_DBG_VERBOSE(...)     DUMMY_FUNCTION()
-#endif
-
-#define MCDRV_DBG(txt, ...) \
-	printk(KERN_INFO "mcDrvKMod [%d on CPU%d] %s(): " txt, \
-		task_pid_vnr(current), \
-		raw_smp_processor_id(), \
-		__func__, \
-		##__VA_ARGS__)
-
-#define MCDRV_DBG_WARN(txt, ...) \
-	printk(KERN_WARNING "mcDrvKMod [%d] %s() WARNING: " txt, \
-		task_pid_vnr(current), \
-		__func__, \
-		##__VA_ARGS__)
-
-#define MCDRV_ASSERT(cond) \
-	do { \
-		if (unlikely(!(cond))) { \
-			panic("mcDrvKMod Assertion failed: %s:%d\n", \
-				__FILE__, __LINE__); \
-		} \
-	} while (0)
-
-#else
-
-#define MCDRV_DBG_VERBOSE(...)	DUMMY_FUNCTION()
-#define MCDRV_DBG(...)		DUMMY_FUNCTION()
-#define MCDRV_DBG_WARN(...)	DUMMY_FUNCTION()
-
-#define MCDRV_ASSERT(...)	DUMMY_FUNCTION()
-
-#endif /* [not] defined(DEBUG) */
-
-
-#endif /* _MC_DRV_KMOD_H_ */
-/** @} */
diff --git a/drivers/gud/mobicore_driver/mc_drv_module_android.h b/drivers/gud/mobicore_driver/mc_drv_module_android.h
deleted file mode 100644
index 319509f..0000000
--- a/drivers/gud/mobicore_driver/mc_drv_module_android.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/**
- * Header file of MobiCore Driver Kernel Module.
- *
- * @addtogroup MobiCore_Driver_Kernel_Module
- * @{
- * Android specific defines
- * @file
- *
- * Android specific defines
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef _MC_DRV_MODULE_ANDROID_H_
-#define _MC_DRV_MODULE_ANDROID_H_
-
-/* Defines needed to identify the Daemon in Android systems
- * For the full list see:
- * platform_system_core/include/private/android_filesystem_config.h in the
- * Android source tree
- */
-/* traditional unix root user */
-#define AID_ROOT	0
-/* system server */
-#define AID_SYSTEM	1000
-/* access to misc storage */
-#define AID_MISC	9998
-#define AID_NOBODY	9999
-/* first app user */
-#define AID_APP		10000
-
-#endif /* _MC_DRV_MODULE_ANDROID_H_ */
-/** @} */
diff --git a/drivers/gud/mobicore_driver/mc_drv_module_fastcalls.h b/drivers/gud/mobicore_driver/mc_drv_module_fastcalls.h
deleted file mode 100644
index d058043..0000000
--- a/drivers/gud/mobicore_driver/mc_drv_module_fastcalls.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/**
- * Header file of MobiCore Driver Kernel Module.
- *
- * @addtogroup MobiCore_Driver_Kernel_Module
- * @{
- * Internal structures of the McDrvModule
- * @file
- *
- * MobiCore Fast Call interface
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef _MC_DRV_MODULE_FC_H_
-#define _MC_DRV_MODULE_FC_H_
-
-#include "mc_drv_module.h"
-
-/**
- * MobiCore SMCs
- */
-enum mc_smc_codes {
-	MC_SMC_N_YIELD  = 0x3, /**< Yield to switch from NWd to SWd. */
-	MC_SMC_N_SIQ    = 0x4  /**< SIQ to switch from NWd to SWd. */
-};
-
-/**
- * MobiCore fast calls. See MCI documentation
- */
-enum mc_fast_call_codes {
-	MC_FC_INIT      = -1,
-	MC_FC_INFO      = -2,
-	MC_FC_POWER     = -3,
-	MC_FC_DUMP      = -4,
-	MC_FC_NWD_TRACE = -31 /**< Mem trace setup fastcall */
-};
-
-/**
- * return code for fast calls
- */
-enum mc_fast_calls_result {
-	MC_FC_RET_OK                       = 0,
-	MC_FC_RET_ERR_INVALID              = 1,
-	MC_FC_RET_ERR_ALREADY_INITIALIZED  = 5
-};
-
-
-
-/*------------------------------------------------------------------------------
-	structure wrappers for specific fastcalls
-------------------------------------------------------------------------------*/
-
-/** generic fast call parameters */
-union fc_generic {
-	struct {
-		uint32_t cmd;
-		uint32_t param[3];
-	} as_in;
-	struct {
-		uint32_t resp;
-		uint32_t ret;
-		uint32_t param[2];
-	} as_out;
-};
-
-
-/** fast call init */
-union mc_fc_init {
-	union fc_generic as_generic;
-	struct {
-		uint32_t cmd;
-		uint32_t base;
-		uint32_t nq_info;
-		uint32_t mcp_info;
-	} as_in;
-	struct {
-		uint32_t resp;
-		uint32_t ret;
-		uint32_t rfu[2];
-	} as_out;
-};
-
-
-/** fast call info parameters */
-union mc_fc_info {
-	union fc_generic as_generic;
-	struct {
-		uint32_t cmd;
-		uint32_t ext_info_id;
-		uint32_t rfu[2];
-	} as_in;
-	struct {
-		uint32_t resp;
-		uint32_t ret;
-		uint32_t state;
-		uint32_t ext_info;
-	} as_out;
-};
-
-
-/** fast call S-Yield parameters */
-union mc_fc_s_yield {
-	union fc_generic as_generic;
-	struct {
-		uint32_t cmd;
-		uint32_t rfu[3];
-	} as_in;
-	struct {
-		uint32_t resp;
-		uint32_t ret;
-		uint32_t rfu[2];
-	} as_out;
-};
-
-
-/** fast call N-SIQ parameters */
-union mc_fc_nsiq {
-	union fc_generic as_generic;
-	struct {
-		uint32_t cmd;
-		uint32_t rfu[3];
-	} as_in;
-	struct {
-		uint32_t resp;
-		uint32_t ret;
-		uint32_t rfu[2];
-	} as_out;
-};
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * fast call to MobiCore
- *
- * @param fc_generic pointer to fast call data
- */
-static inline void mc_fastcall(
-	union fc_generic *fc_generic
-)
-{
-	MCDRV_ASSERT(fc_generic != NULL);
-	/* We only expect to make smc calls on CPU0 otherwise something wrong
-	 * will happen */
-	MCDRV_ASSERT(raw_smp_processor_id() == 0);
-	mb();
-#ifdef MC_SMC_FASTCALL
-	{
-		int ret = 0;
-		MCDRV_DBG("Going into SCM()");
-		ret = smc_fastcall((void *)fc_generic, sizeof(*fc_generic));
-		MCDRV_DBG("Coming from SCM, scm_call=%i, resp=%d/0x%x\n",
-			ret,
-			fc_generic->as_out.resp, fc_generic->as_out.resp);
-	}
-#else
-	{
-		/* SVC expect values in r0-r3 */
-		register u32 reg0 __asm__("r0") = fc_generic->as_in.cmd;
-		register u32 reg1 __asm__("r1") = fc_generic->as_in.param[0];
-		register u32 reg2 __asm__("r2") = fc_generic->as_in.param[1];
-		register u32 reg3 __asm__("r3") = fc_generic->as_in.param[2];
-
-		/* one of the famous preprocessor hacks to stingitize things.*/
-#define __STR2(x)   #x
-#define __STR(x)    __STR2(x)
-
-		/* compiler does not support certain instructions
-		"SMC": secure monitor call.*/
-#define ASM_ARM_SMC         0xE1600070
-		/*   "BPKT": debugging breakpoint. We keep this, as is comes
-				quite handy for debugging. */
-#define ASM_ARM_BPKT        0xE1200070
-#define ASM_THUMB_BPKT      0xBE00
-
-
-		__asm__ volatile (
-			".word " __STR(ASM_ARM_SMC) "\n"
-			: "+r"(reg0), "+r"(reg1), "+r"(reg2), "+r"(reg3)
-		);
-
-		/* set response */
-		fc_generic->as_out.resp     = reg0;
-		fc_generic->as_out.ret      = reg1;
-		fc_generic->as_out.param[0] = reg2;
-		fc_generic->as_out.param[1] = reg3;
-	}
-#endif
-}
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * convert fast call return code to linux driver module error code
- *
- */
-static inline int convert_fc_ret(
-	uint32_t sret
-)
-{
-	int         ret = -EFAULT;
-
-	switch (sret) {
-
-	case MC_FC_RET_OK:
-		ret = 0;
-		break;
-
-	case MC_FC_RET_ERR_INVALID:
-		ret = -EINVAL;
-		break;
-
-	case MC_FC_RET_ERR_ALREADY_INITIALIZED:
-		ret = -EBUSY;
-		break;
-
-	default:
-		break;
-	} /* end switch( sret ) */
-	return ret;
-}
-
-#endif /* _MC_DRV_MODULE_FC_H_ */
-/** @} */
diff --git a/drivers/gud/mobicore_driver/mc_drv_module_linux_api.h b/drivers/gud/mobicore_driver/mc_drv_module_linux_api.h
deleted file mode 100644
index b2a99f1..0000000
--- a/drivers/gud/mobicore_driver/mc_drv_module_linux_api.h
+++ /dev/null
@@ -1,187 +0,0 @@
-/**
- * Header file of MobiCore Driver Kernel Module.
- *
- * @addtogroup MobiCore_Driver_Kernel_Module
- * @{
- * Wrapper for Linux API
- * @file
- *
- * Some convenient wrappers for memory functions
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#ifndef _MC_DRV_MODULE_LINUX_API_H_
-#define _MC_DRV_MODULE_LINUX_API_H_
-
-#include <linux/version.h>
-#include <linux/miscdevice.h>
-#include <linux/interrupt.h>
-#include <linux/highmem.h>
-#include <linux/kthread.h>
-#include <linux/io.h>
-#include <linux/uaccess.h>
-#include <asm/sizes.h>
-#include <asm/pgtable.h>
-#include <linux/semaphore.h>
-#include <linux/moduleparam.h>
-#include <linux/slab.h>
-
-
-/* make some nice types */
-#if !defined(TRUE)
-#define TRUE (1 == 1)
-#endif
-
-#if !defined(FALSE)
-#define FALSE (1 != 1)
-#endif
-
-
-/* Linux GCC modifiers */
-#if !defined(__init)
-#warning "missing definition: __init"
-/* define a dummy */
-#define __init
-#endif
-
-
-#if !defined(__exit)
-#warning "missing definition: __exit"
-/* define a dummy */
-#define __exit
-#endif
-
-
-#if !defined(__must_check)
-#warning "missing definition: __must_check"
-/* define a dummy */
-#define __must_check
-#endif
-
-
-#if !defined(__user)
-#warning "missing definition: __user"
-/* define a dummy */
-#define __user
-#endif
-
-#define INVALID_ORDER       ((unsigned int)(-1))
-
-/*----------------------------------------------------------------------------*/
-/* get start address of the 4 KiB page where the given addres is located in. */
-static inline void *get_page_start(
-	void *addr
-)
-{
-	return (void *)(((unsigned long)(addr)) & PAGE_MASK);
-}
-
-/*----------------------------------------------------------------------------*/
-/* get offset into the 4 KiB page where the given addres is located in. */
-static inline unsigned int get_offset_in_page(
-	void *addr
-)
-{
-	return (unsigned int)(((unsigned long)(addr)) & (~PAGE_MASK));
-}
-
-/*----------------------------------------------------------------------------*/
-/* get number of pages for a given buffer. */
-static inline unsigned int get_nr_of_pages_for_buffer(
-	void		*addr_start, /* may be null */
-	unsigned int	len
-)
-{
-	/* calculate used number of pages. Example:
-	offset+size    newSize+PAGE_SIZE-1    nr_of_pages
-	   0              4095                   0
-	   1              4096                   1
-	  4095            8190                   1
-	  4096            8191                   1
-	  4097            8192                   2 */
-
-	return (get_offset_in_page(addr_start) + len + PAGE_SIZE-1) / PAGE_SIZE;
-}
-
-
-/*----------------------------------------------------------------------------*/
-/**
- * convert a given size to page order, which is equivalent to finding log_2(x).
- * The maximum for order was 5 in Linux 2.0 corresponding to 32 pages.
- * Later versions allow 9 corresponding to 512 pages, which is 2 MB on
- * most platforms). Anyway, the bigger order is, the more likely it is
- * that the allocation will fail.
- * Size       0           1  4097  8193  12289  24577  28673   40961   61441
- * Pages      -           1     2     3      4      7      8      15      16
- * Order  INVALID_ORDER   0     1     1      2      2      3       3       4
- *
- * @param  size
- * @return order
- */
-static inline unsigned int size_to_order(
-	unsigned int size
-)
-{
-	unsigned int order = INVALID_ORDER;
-
-	if (size != 0) {
-		/* ARMv5 as a CLZ instruction which count the leading zeros of
-		the binary representation of a value. It return a value
-		between 0 and 32.
-		Value   0   1   2   3   4   5   6   7   8   9  10 ...
-		CLZ    32  31  30  30  29  29  29  29  28  28  28 ...
-
-		We have excluded Size==0 before, so this is safe. */
-		order = __builtin_clz(
-				get_nr_of_pages_for_buffer(NULL, size));
-
-		/* there is a size overflow in get_nr_of_pages_for_buffer when
-		 * the size is too large */
-		if (unlikely(order > 31))
-			return INVALID_ORDER;
-		order = 31 - order;
-
-		/* above algorithm rounds down: clz(5)=2 instead of 3 */
-		/* quick correction to fix it: */
-		if (((1<<order)*PAGE_SIZE) < size)
-			order++;
-	}
-	return order;
-}
-
-/* magic linux macro */
-#if !defined(list_for_each_entry)
-/* stop compiler */
-#error "missing macro: list_for_each_entry()"
-/* define a dummy */
-#define list_for_each_entry(a, b, c)    if (0)
-#endif
-
-/*----------------------------------------------------------------------------*/
-/* return the page frame number of an address */
-static inline unsigned int addr_to_pfn(
-	void *addr
-)
-{
-	/* there is no real API for this */
-	return ((unsigned int)(addr)) >> PAGE_SHIFT;
-}
-
-
-/*----------------------------------------------------------------------------*/
-/* return the address of a page frame number */
-static inline void *pfn_to_addr(
-	unsigned int pfn
-)
-{
-	/* there is no real API for this */
-	return (void *)(pfn << PAGE_SHIFT);
-}
-
-#endif /* _MC_DRV_MODULE_LINUX_API_H_ */
-/** @} */
diff --git a/drivers/gud/mobicore_driver/mem.c b/drivers/gud/mobicore_driver/mem.c
new file mode 100644
index 0000000..da711ce
--- /dev/null
+++ b/drivers/gud/mobicore_driver/mem.c
@@ -0,0 +1,696 @@
+/*
+ * MobiCore Driver Kernel Module.
+ *
+ * This module is written as a Linux device driver.
+ * This driver represents the command proxy on the lowest layer, from the
+ * secure world to the non secure world, and vice versa.
+ * This driver is located in the non secure world (Linux).
+ * This driver offers IOCTL commands, for access to the secure world, and has
+ * the interface from the secure world to the normal world.
+ * The access to the driver is possible with a file descriptor,
+ * which has to be created by the fd = open(/dev/mobicore) command.
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#include "main.h"
+#include "debug.h"
+#include "mem.h"
+
+#include <linux/highmem.h>
+#include <linux/slab.h>
+#include <linux/kthread.h>
+#include <linux/pagemap.h>
+#include <linux/device.h>
+
+
+/* MobiCore memory context data */
+struct mc_mem_context mem_ctx;
+
+/* convert L2 PTE to page pointer */
+static inline struct page *l2_pte_to_page(pte_t pte)
+{
+	unsigned long phys_page_addr = ((unsigned long)pte & PAGE_MASK);
+	unsigned int pfn = phys_page_addr >> PAGE_SHIFT;
+	struct page *page = pfn_to_page(pfn);
+	return page;
+}
+
+/* convert page pointer to L2 PTE */
+static inline pte_t page_to_l2_pte(struct page *page)
+{
+	unsigned long pfn = page_to_pfn(page);
+	unsigned long phys_addr = (pfn << PAGE_SHIFT);
+	pte_t pte = (pte_t)(phys_addr & PAGE_MASK);
+	return pte;
+}
+
+static inline void release_page(struct page *page)
+{
+	SetPageDirty(page);
+
+	page_cache_release(page);
+}
+
+static int lock_pages(struct task_struct *task, void *virt_start_page_addr,
+	int pages_no, struct page **pages)
+{
+	int locked_pages;
+
+	/* lock user pages, must hold the mmap_sem to do this. */
+	down_read(&(task->mm->mmap_sem));
+	locked_pages = get_user_pages(
+				task,
+				task->mm,
+				(unsigned long)virt_start_page_addr,
+				pages_no,
+				1, /* write access */
+				0,
+				pages,
+				NULL);
+	up_read(&(task->mm->mmap_sem));
+
+	/* check if we could lock all pages. */
+	if (locked_pages != pages_no) {
+		MCDRV_DBG_ERROR(mcd, "get_user_pages() failed, locked_pages=%d",
+				locked_pages);
+		if (locked_pages > 0) {
+			/* release all locked pages. */
+			release_pages(pages, locked_pages, 0);
+		}
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+/* Get kernel pointer to shared L2 table given a per-process reference */
+struct l2table *get_l2_table_kernel_virt(struct mc_l2_table *table)
+{
+	if (WARN(!table, "Invalid L2 table"))
+		return NULL;
+
+	if (WARN(!table->set, "Invalid L2 table set"))
+		return NULL;
+
+	if (WARN(!table->set->kernel_virt, "Invalid L2 pointer"))
+		return NULL;
+
+	return &(table->set->kernel_virt->table[table->idx]);
+}
+
+/* Get physical address of a shared L2 table given a per-process reference */
+struct l2table *get_l2_table_phys(struct mc_l2_table *table)
+{
+	if (WARN(!table, "Invalid L2 table"))
+		return NULL;
+	if (WARN(!table->set, "Invalid L2 table set"))
+		return NULL;
+	if (WARN(!table->set->kernel_virt, "Invalid L2 phys pointer"))
+		return NULL;
+
+	return &(table->set->phys->table[table->idx]);
+}
+
+static inline int in_use(struct mc_l2_table *table)
+{
+	return atomic_read(&table->usage) > 0;
+}
+
+/*
+ * Search the list of used l2 tables and return the one with the handle.
+ * Assumes the table_lock is taken.
+ */
+struct mc_l2_table *find_l2_table(unsigned int handle)
+{
+	struct mc_l2_table *table;
+
+	list_for_each_entry(table, &mem_ctx.l2_tables, list) {
+		if (table->handle == handle)
+			return table;
+	}
+	return NULL;
+}
+
+/*
+ * Allocate a new l2 table store plus L2_TABLES_PER_PAGE in the l2 free tables
+ * list. Assumes the table_lock is already taken by the caller above.
+ */
+static int alloc_table_store(void)
+{
+	unsigned long store;
+	struct mc_l2_tables_set *l2table_set;
+	struct mc_l2_table *l2table, *l2table2;
+	struct page *page;
+	int ret = 0, i;
+	/* temp list for holding the l2 tables */
+	LIST_HEAD(temp);
+
+	store = get_zeroed_page(GFP_KERNEL);
+	if (!store)
+		return -ENOMEM;
+
+	/*
+	 * Actually, locking is not necessary, because kernel
+	 * memory is not supposed to get swapped out. But we
+	 * play safe....
+	 */
+	page = virt_to_page(store);
+	SetPageReserved(page);
+
+	/* add all the descriptors to the free descriptors list */
+	l2table_set = kmalloc(sizeof(*l2table_set), GFP_KERNEL | __GFP_ZERO);
+	if (l2table_set == NULL) {
+		ret = -ENOMEM;
+		goto free_store;
+	}
+	/* initialize */
+	l2table_set->kernel_virt = (void *)store;
+	l2table_set->page = page;
+	l2table_set->phys = (void *)virt_to_phys((void *)store);
+	/* the set is not yet used */
+	atomic_set(&l2table_set->used_tables, 0);
+
+	/* init add to list. */
+	INIT_LIST_HEAD(&(l2table_set->list));
+	list_add(&l2table_set->list, &mem_ctx.l2_tables_sets);
+
+	for (i = 0; i < L2_TABLES_PER_PAGE; i++) {
+		/* allocate a WSM L2 descriptor */
+		l2table  = kmalloc(sizeof(*l2table), GFP_KERNEL | __GFP_ZERO);
+		if (l2table == NULL) {
+			ret = -ENOMEM;
+			MCDRV_DBG_ERROR(mcd, "out of memory\n");
+			/* Free the full temp list and the store in this case */
+			goto free_temp_list;
+		}
+
+		/* set set reference */
+		l2table->set = l2table_set;
+		l2table->idx = i;
+		l2table->virt = get_l2_table_kernel_virt(l2table);
+		l2table->phys = (unsigned long)get_l2_table_phys(l2table);
+		atomic_set(&l2table->usage, 0);
+
+		/* add to temp list. */
+		INIT_LIST_HEAD(&l2table->list);
+		list_add_tail(&l2table->list, &temp);
+	}
+
+	/*
+	 * If everything went ok then merge the temp list with the global
+	 * free list
+	 */
+	list_splice_tail(&temp, &mem_ctx.free_l2_tables);
+	return 0;
+free_temp_list:
+	list_for_each_entry_safe(l2table, l2table2, &temp, list) {
+		kfree(l2table);
+	}
+
+	list_del(&l2table_set->list);
+
+free_store:
+	free_page(store);
+	return ret;
+
+}
+/*
+ * Get a l2 table from the free tables list or allocate a new one and
+ * initialize it. Assumes the table_lock is already taken.
+ */
+static struct mc_l2_table *alloc_l2_table(struct mc_instance *instance)
+{
+	int ret = 0;
+	struct mc_l2_table *table = NULL;
+
+	if (list_empty(&mem_ctx.free_l2_tables)) {
+		ret = alloc_table_store();
+		if (ret) {
+			MCDRV_DBG_ERROR(mcd, "Failed to allocate new store!");
+			return ERR_PTR(-ENOMEM);
+		}
+		/* if it's still empty something wrong has happened */
+		if (list_empty(&mem_ctx.free_l2_tables)) {
+			MCDRV_DBG_ERROR(mcd,
+					"Free list not updated correctly!");
+			return ERR_PTR(-EFAULT);
+		}
+	}
+
+	/* get a WSM L2 descriptor */
+	table  = list_first_entry(&mem_ctx.free_l2_tables,
+		struct mc_l2_table, list);
+	if (table == NULL) {
+		MCDRV_DBG_ERROR(mcd, "out of memory\n");
+		return ERR_PTR(-ENOMEM);
+	}
+	/* Move it to the used l2 tables list */
+	list_move_tail(&table->list, &mem_ctx.l2_tables);
+
+	table->handle = get_unique_id();
+	table->owner = instance;
+
+	atomic_inc(&table->set->used_tables);
+	atomic_inc(&table->usage);
+
+	MCDRV_DBG_VERBOSE(mcd,
+			  "chunkPhys=%p,idx=%d", table->set->phys, table->idx);
+
+	return table;
+}
+
+/*
+ * Frees the object associated with a l2 table. Initially the object is moved
+ * to the free tables list, but if all the 4 lists of the store are free
+ * then the store is also released.
+ * Assumes the table_lock is already taken.
+ */
+static void free_l2_table(struct mc_l2_table *table)
+{
+	struct mc_l2_tables_set *l2table_set;
+
+	if (WARN(!table, "Invalid table"))
+		return;
+
+	l2table_set = table->set;
+	if (WARN(!l2table_set, "Invalid table set"))
+		return;
+
+	list_move_tail(&table->list, &mem_ctx.free_l2_tables);
+
+	/* if nobody uses this set, we can release it. */
+	if (atomic_dec_and_test(&l2table_set->used_tables)) {
+		struct mc_l2_table *tmp;
+
+		/* remove from list */
+		list_del(&l2table_set->list);
+		/*
+		 * All the l2 tables are in the free list for this set
+		 * so we can just remove them from there
+		 */
+		list_for_each_entry_safe(table, tmp, &mem_ctx.free_l2_tables,
+					 list) {
+			if (table->set == l2table_set) {
+				list_del(&table->list);
+				kfree(table);
+			}
+		} /* end while */
+
+		/*
+		 * We shouldn't recover from this since it was some data
+		 * corruption before
+		 */
+		BUG_ON(!l2table_set->page);
+		ClearPageReserved(l2table_set->page);
+
+		BUG_ON(!l2table_set->kernel_virt);
+		free_page((unsigned long)l2table_set->kernel_virt);
+
+		kfree(l2table_set);
+	}
+}
+
+/*
+ * Create a L2 table in a WSM container that has been allocates previously.
+ * Assumes the table lock is already taken or there is no need to take like
+ * when first creating the l2 table the full list is locked.
+ *
+ * @task	pointer to task owning WSM
+ * @wsm_buffer	user space WSM start
+ * @wsm_len	WSM length
+ * @table	Pointer to L2 table details
+ */
+static int map_buffer(struct task_struct *task, void *wsm_buffer,
+		      unsigned int wsm_len, struct mc_l2_table *table)
+{
+	int		ret = 0;
+	unsigned int	i, nr_of_pages;
+	/* start address of the 4 KiB page of wsm_buffer */
+	void		*virt_addr_page;
+	struct page	*page;
+	struct l2table	*l2table;
+	struct page	**l2table_as_array_of_pointers_to_page;
+	/* page offset in wsm buffer */
+	unsigned int offset;
+
+	if (WARN(!wsm_buffer, "Invalid WSM buffer pointer"))
+		return -EINVAL;
+
+	if (WARN(wsm_len == 0, "Invalid WSM buffer length"))
+		return -EINVAL;
+
+	if (WARN(!table, "Invalid mapping table for WSM"))
+		return -EINVAL;
+
+	/* no size > 1Mib supported */
+	if (wsm_len > SZ_1M) {
+		MCDRV_DBG_ERROR(mcd, "size > 1 MiB\n");
+		return -EINVAL;
+	}
+
+	MCDRV_DBG_VERBOSE(mcd, "WSM addr=0x%p, len=0x%08x\n", wsm_buffer,
+			  wsm_len);
+
+	/*
+	 * Check if called from kernel space and if
+	 * wsm_buffer is actually vmalloced or not
+	 */
+	if (task == NULL && !is_vmalloc_addr(wsm_buffer)) {
+		MCDRV_DBG_ERROR(mcd, "WSM addr is not a vmalloc address");
+		return -EINVAL;
+	}
+
+	/* calculate page usage */
+	virt_addr_page = (void *)(((unsigned long)(wsm_buffer)) & PAGE_MASK);
+	offset = (unsigned int)	(((unsigned long)(wsm_buffer)) & (~PAGE_MASK));
+	nr_of_pages  = PAGE_ALIGN(offset + wsm_len) / PAGE_SIZE;
+
+	MCDRV_DBG_VERBOSE(mcd, "virt addr page start=0x%p, pages=%d\n",
+			  virt_addr_page, nr_of_pages);
+
+	/* L2 table can hold max 1MiB in 256 pages. */
+	if ((nr_of_pages * PAGE_SIZE) > SZ_1M) {
+		MCDRV_DBG_ERROR(mcd, "WSM paged exceed 1 MiB\n");
+		return -EINVAL;
+	}
+
+	l2table = table->virt;
+	/*
+	 * We use the memory for the L2 table to hold the pointer
+	 * and convert them later. This works, as everything comes
+	 * down to a 32 bit value.
+	 */
+	l2table_as_array_of_pointers_to_page = (struct page **)l2table;
+
+	/* Request comes from user space */
+	if (task != NULL && !is_vmalloc_addr(wsm_buffer)) {
+		/*
+		 * lock user page in memory, so they do not get swapped
+		 * out.
+		 * REV axh: Kernel 2.6.27 added a new get_user_pages_fast()
+		 * function, maybe it is called fast_gup() in some versions.
+		 * handle user process doing a fork().
+		 * Child should not get things.
+		 * http://osdir.com/ml/linux-media/2009-07/msg00813.html
+		 * http://lwn.net/Articles/275808/
+		 */
+		ret = lock_pages(task, virt_addr_page, nr_of_pages,
+				 l2table_as_array_of_pointers_to_page);
+		if (ret != 0) {
+			MCDRV_DBG_ERROR(mcd, "lock_user_pages() failed\n");
+			return ret;
+		}
+	}
+	/* Request comes from kernel space(vmalloc buffer) */
+	else {
+		void *uaddr = wsm_buffer;
+		for (i = 0; i < nr_of_pages; i++) {
+			page = vmalloc_to_page(uaddr);
+			if (!page) {
+				MCDRV_DBG_ERROR(mcd, "failed to map address");
+				return -EINVAL;
+			}
+			get_page(page);
+			l2table_as_array_of_pointers_to_page[i] = page;
+			uaddr += PAGE_SIZE;
+		}
+	}
+
+	table->pages = nr_of_pages;
+
+	/*
+	 * create L2 Table entries.
+	 * used_l2table->table contains a list of page pointers here.
+	 * For a proper cleanup we have to ensure that the following
+	 * code either works and used_l2table contains a valid L2 table
+	 * - or fails and used_l2table->table contains the list of page
+	 * pointers.
+	 * Any mixed contents will make cleanup difficult.
+	 */
+	for (i = 0; i < nr_of_pages; i++) {
+		pte_t pte;
+		page = l2table_as_array_of_pointers_to_page[i];
+
+		/*
+		 * create L2 table entry, see ARM MMU docu for details
+		 * about flags stored in the lowest 12 bits.
+		 * As a side reference, the Article
+		 * "ARM's multiply-mapped memory mess"
+		 * found in the collection at
+		 * http://lwn.net/Articles/409032/
+		 * is also worth reading.
+		 */
+		pte = page_to_l2_pte(page)
+				| PTE_EXT_AP1 | PTE_EXT_AP0
+				| PTE_CACHEABLE | PTE_BUFFERABLE
+				| PTE_TYPE_SMALL | PTE_TYPE_EXT | PTE_EXT_NG;
+		/*
+		 * Linux uses different mappings for SMP systems(the
+		 * sharing flag is set for the pte. In order not to
+		 * confuse things too much in Mobicore make sure the
+		 * shared buffers have the same flags.
+		 * This should also be done in SWD side
+		 */
+#ifdef CONFIG_SMP
+		pte |= PTE_EXT_SHARED | PTE_EXT_TEX(1);
+#endif
+
+		l2table->table_entries[i] = pte;
+		MCDRV_DBG_VERBOSE(mcd, "L2 entry %d:  0x%08x\n", i,
+				  (unsigned int)(pte));
+	}
+
+	/* ensure rest of table is empty */
+	while (i < 255)
+		l2table->table_entries[i++] = (pte_t)0;
+
+
+	return ret;
+}
+
+/*
+ * Remove a L2 table in a WSM container. Afterwards the container may be
+ * released. Assumes the table_lock and the lock is taken.
+ */
+static void unmap_buffers(struct mc_l2_table *table)
+{
+	struct l2table *l2table;
+	int i;
+
+	if (WARN_ON(!table))
+		return;
+
+	/* found the table, now release the resources. */
+	MCDRV_DBG_VERBOSE(mcd, "clear L2 table, phys_base=%p, nr_of_pages=%d\n",
+			  (void *)table->phys, table->pages);
+
+	l2table = table->virt;
+
+	/* release all locked user space pages */
+	for (i = 0; i < table->pages; i++) {
+		/* convert physical entries from L2 table to page pointers */
+		pte_t pte = l2table->table_entries[i];
+		struct page *page = l2_pte_to_page(pte);
+		release_page(page);
+	}
+
+	/* remember that all pages have been freed */
+	table->pages = 0;
+}
+
+/* Delete a used l2 table. Assumes the table_lock and the lock is taken */
+static void unmap_l2_table(struct mc_l2_table *table)
+{
+	/* Check if it's not locked by other processes too! */
+	if (!atomic_dec_and_test(&table->usage))
+		return;
+
+	/* release if Nwd and Swd/MC do no longer use it. */
+	unmap_buffers(table);
+	free_l2_table(table);
+}
+
+int mc_free_l2_table(struct mc_instance *instance, uint32_t handle)
+{
+	struct mc_l2_table *table;
+	int ret = 0;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	mutex_lock(&mem_ctx.table_lock);
+	table = find_l2_table(handle);
+
+	if (table == NULL) {
+		MCDRV_DBG_VERBOSE(mcd, "entry not found");
+		ret = -EINVAL;
+		goto err_unlock;
+	}
+	if (instance != table->owner && !is_daemon(instance)) {
+		MCDRV_DBG_ERROR(mcd, "instance does no own it");
+		ret = -EPERM;
+		goto err_unlock;
+	}
+	/* free table (if no further locks exist) */
+	unmap_l2_table(table);
+err_unlock:
+	mutex_unlock(&mem_ctx.table_lock);
+
+	return ret;
+}
+
+int mc_lock_l2_table(struct mc_instance *instance, uint32_t handle)
+{
+	int ret = 0;
+	struct mc_l2_table *table = NULL;
+
+	if (WARN(!instance, "No instance data available"))
+		return -EFAULT;
+
+	mutex_lock(&mem_ctx.table_lock);
+	table = find_l2_table(handle);
+
+	if (table == NULL) {
+		MCDRV_DBG_VERBOSE(mcd, "entry not found %u\n", handle);
+		ret = -EINVAL;
+		goto table_err;
+	}
+	if (instance != table->owner && !is_daemon(instance)) {
+		MCDRV_DBG_ERROR(mcd, "instance does no own it\n");
+		ret = -EPERM;
+		goto table_err;
+	}
+
+	/* lock entry */
+	atomic_inc(&table->usage);
+table_err:
+	mutex_unlock(&mem_ctx.table_lock);
+	return ret;
+}
+/*
+ * Allocate L2 table and map buffer into it.
+ * That is, create respective table entries.
+ * Must hold Semaphore mem_ctx.wsm_l2_sem
+ */
+struct mc_l2_table *mc_alloc_l2_table(struct mc_instance *instance,
+	struct task_struct *task, void *wsm_buffer, unsigned int wsm_len)
+{
+	int ret = 0;
+	struct mc_l2_table *table;
+
+	if (WARN(!instance, "No instance data available"))
+		return ERR_PTR(-EFAULT);
+
+	mutex_lock(&mem_ctx.table_lock);
+	table = alloc_l2_table(instance);
+	if (IS_ERR(table)) {
+		MCDRV_DBG_ERROR(mcd, "allocate_used_l2_table() failed\n");
+		ret = -ENOMEM;
+		goto err_no_mem;
+	}
+
+	/* create the L2 page for the WSM */
+	ret = map_buffer(task, wsm_buffer, wsm_len, table);
+
+	if (ret != 0) {
+		MCDRV_DBG_ERROR(mcd, "map_buffer() failed\n");
+		unmap_l2_table(table);
+		goto err_no_mem;
+	}
+	MCDRV_DBG(mcd, "mapped buffer %p to table with handle %d @ %lx",
+		  wsm_buffer, table->handle, table->phys);
+
+	mutex_unlock(&mem_ctx.table_lock);
+	return table;
+err_no_mem:
+	mutex_unlock(&mem_ctx.table_lock);
+	return ERR_PTR(ret);
+}
+
+uint32_t mc_find_l2_table(struct mc_instance *instance, uint32_t handle)
+{
+	uint32_t ret = 0;
+	struct mc_l2_table *table = NULL;
+
+	if (WARN(!instance, "No instance data available"))
+		return 0;
+
+	mutex_lock(&mem_ctx.table_lock);
+	table = find_l2_table(handle);
+
+	if (table == NULL) {
+		MCDRV_DBG_ERROR(mcd, "entry not found %u\n", handle);
+		ret = 0;
+		goto table_err;
+	}
+
+	ret = table->phys;
+table_err:
+	mutex_unlock(&mem_ctx.table_lock);
+	return ret;
+}
+
+void mc_clean_l2_tables(void)
+{
+	struct mc_l2_table *table, *tmp;
+
+	mutex_lock(&mem_ctx.table_lock);
+	/* Check if some WSM is orphaned. */
+	list_for_each_entry_safe(table, tmp, &mem_ctx.l2_tables, list) {
+		if (table->owner == NULL) {
+			MCDRV_DBG(mcd,
+				  "clearing orphaned WSM L2: p=%lx pages=%d\n",
+				  table->phys, table->pages);
+			unmap_l2_table(table);
+		}
+	}
+	mutex_unlock(&mem_ctx.table_lock);
+}
+
+void mc_clear_l2_tables(struct mc_instance *instance)
+{
+	struct mc_l2_table *table, *tmp;
+
+	mutex_lock(&mem_ctx.table_lock);
+	/* Check if some WSM is still in use. */
+	list_for_each_entry_safe(table, tmp, &mem_ctx.l2_tables, list) {
+		if (table->owner == instance) {
+			MCDRV_DBG(mcd, "release WSM L2: p=%lx pages=%d\n",
+				  table->phys, table->pages);
+			/* unlock app usage and free or mark it as orphan */
+			table->owner = NULL;
+			unmap_l2_table(table);
+		}
+	}
+	mutex_unlock(&mem_ctx.table_lock);
+}
+
+int mc_init_l2_tables(void)
+{
+	/* init list for WSM L2 chunks. */
+	INIT_LIST_HEAD(&mem_ctx.l2_tables_sets);
+
+	/* L2 table descriptor list. */
+	INIT_LIST_HEAD(&mem_ctx.l2_tables);
+
+	/* L2 table descriptor list. */
+	INIT_LIST_HEAD(&mem_ctx.free_l2_tables);
+
+	mutex_init(&mem_ctx.table_lock);
+
+	return 0;
+}
+
+void mc_release_l2_tables()
+{
+	struct mc_l2_table *table;
+	/* Check if some WSM is still in use. */
+	list_for_each_entry(table, &mem_ctx.l2_tables, list) {
+		WARN(1, "WSM L2 still in use: phys=%lx ,nr_of_pages=%d\n",
+		     table->phys, table->pages);
+	}
+}
diff --git a/drivers/gud/mobicore_driver/mem.h b/drivers/gud/mobicore_driver/mem.h
new file mode 100644
index 0000000..a90662a7
--- /dev/null
+++ b/drivers/gud/mobicore_driver/mem.h
@@ -0,0 +1,127 @@
+/*
+ * MobiCore driver module.(interface to the secure world SWD)
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_MEM_H_
+#define _MC_MEM_H_
+
+#define FREE_FROM_SWD	1
+#define FREE_FROM_NWD	0
+
+#define LOCKED_BY_APP	(1U << 0)
+#define LOCKED_BY_MC	(1U << 1)
+
+/*
+ * MobiCore specific page tables for world shared memory.
+ * Linux uses shadow page tables, see arch/arm/include/asm/pgtable-2level.
+ * MobiCore uses the default ARM format.
+ *
+ * Number of page table entries in one L2 table. This is ARM specific, an
+ * L2 table covers 1 MiB by using 256 entry referring to 4KiB pages each.
+ */
+#define MC_ARM_L2_TABLE_ENTRIES		256
+
+/* ARM level 2 (L2) table with 256 entries. Size: 1k */
+struct l2table {
+	pte_t	table_entries[MC_ARM_L2_TABLE_ENTRIES];
+};
+
+/* Number of pages for L2 tables. There are 4 table in each page. */
+#define L2_TABLES_PER_PAGE		4
+
+/* Store for four L2 tables in one 4kb page*/
+struct mc_l2_table_store {
+	struct l2table table[L2_TABLES_PER_PAGE];
+};
+
+/* Usage and maintenance information about mc_l2_table_store */
+struct mc_l2_tables_set {
+	struct list_head		list;
+	/* kernel virtual address */
+	struct mc_l2_table_store	*kernel_virt;
+	/* physical address */
+	struct mc_l2_table_store	*phys;
+	/* pointer to page struct */
+	struct page			*page;
+	/* How many pages from this set are used */
+	atomic_t			used_tables;
+};
+
+/*
+ * L2 table allocated to the Daemon or a TLC describing a world shared buffer.
+ * When users map a malloc()ed area into SWd, a L2 table is allocated.
+ * In addition, the area of maximum 1MB virtual address space is mapped into
+ * the L2 table and a handle for this table is returned to the user.
+ */
+struct mc_l2_table {
+	struct list_head	list;
+	/* Table lock */
+	struct mutex		lock;
+	/* handle as communicated to user mode */
+	unsigned int		handle;
+	/* Number of references kept to this l2 table */
+	atomic_t		usage;
+	/* owner of this L2 table */
+	struct mc_instance	*owner;
+	/* set describing where our L2 table is stored */
+	struct mc_l2_tables_set	*set;
+	/* index into L2 table set */
+	unsigned int		idx;
+	/* size of buffer */
+	unsigned int		pages;
+	/* virtual address*/
+	void			*virt;
+	unsigned long		phys;
+};
+
+/* MobiCore Driver Memory context data. */
+struct mc_mem_context {
+	struct mc_instance	*daemon_inst;
+	/* Backing store for L2 tables */
+	struct list_head	l2_tables_sets;
+	/* Bookkeeping for used L2 tables */
+	struct list_head	l2_tables;
+	/* Bookkeeping for free L2 tables */
+	struct list_head	free_l2_tables;
+	/* semaphore to synchronize access to above lists */
+	struct mutex		table_lock;
+};
+
+/*
+ * Allocate L2 table and map buffer into it.
+ * That is, create respective table entries.
+ */
+struct mc_l2_table *mc_alloc_l2_table(struct mc_instance *instance,
+	struct task_struct *task, void *wsm_buffer, unsigned int wsm_len);
+
+/* Delete all the l2 tables associated with an instance */
+void mc_clear_l2_tables(struct mc_instance *instance);
+
+/* Release all orphaned L2 tables */
+void mc_clean_l2_tables(void);
+
+/* Delete a used l2 table. */
+int mc_free_l2_table(struct mc_instance *instance, uint32_t handle);
+
+/*
+ * Lock a l2 table - the daemon adds +1 to refcount of the L2 table
+ * marking it in use by SWD so it doesn't get released when the TLC dies.
+ */
+int mc_lock_l2_table(struct mc_instance *instance, uint32_t handle);
+/* Unlock l2 table. */
+int mc_unlock_l2_table(struct mc_instance *instance, uint32_t handle);
+/* Return the phys address of l2 table. */
+uint32_t mc_find_l2_table(struct mc_instance *instance, uint32_t handle);
+/* Release all used l2 tables to Linux memory space */
+void mc_release_l2_tables(void);
+
+/* Initialize all l2 tables structure */
+int mc_init_l2_tables(void);
+
+#endif /* _MC_MEM_H_ */
diff --git a/drivers/gud/mobicore_driver/ops.c b/drivers/gud/mobicore_driver/ops.c
new file mode 100644
index 0000000..509b4e9
--- /dev/null
+++ b/drivers/gud/mobicore_driver/ops.c
@@ -0,0 +1,143 @@
+/*
+ * MobiCore Driver Kernel Module.
+ *
+ * This module is written as a Linux device driver.
+ * This driver represents the command proxy on the lowest layer, from the
+ * secure world to the non secure world, and vice versa.
+ * This driver is located in the non secure world (Linux).
+ * This driver offers IOCTL commands, for access to the secure world, and has
+ * the interface from the secure world to the normal world.
+ * The access to the driver is possible with a file descriptor,
+ * which has to be created by the fd = open(/dev/mobicore) command.
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/device.h>
+
+#include "main.h"
+#include "fastcall.h"
+#include "ops.h"
+#include "mem.h"
+#include "debug.h"
+
+int mc_info(uint32_t ext_info_id, uint32_t *state, uint32_t *ext_info)
+{
+	int ret = 0;
+	union mc_fc_info fc_info;
+
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
+
+	memset(&fc_info, 0, sizeof(fc_info));
+	fc_info.as_in.cmd = MC_FC_INFO;
+	fc_info.as_in.ext_info_id = ext_info_id;
+
+	MCDRV_DBG(mcd, "fc_info <- cmd=0x%08x, ext_info_id=0x%08x\n",
+		  fc_info.as_in.cmd, fc_info.as_in.ext_info_id);
+
+	mc_fastcall(&(fc_info.as_generic));
+
+	MCDRV_DBG(mcd,
+		  "fc_info -> r=0x%08x ret=0x%08x state=0x%08x ext_info=0x%08x",
+		  fc_info.as_out.resp,
+		  fc_info.as_out.ret,
+		  fc_info.as_out.state,
+		  fc_info.as_out.ext_info);
+
+	ret = convert_fc_ret(fc_info.as_out.ret);
+
+	*state  = fc_info.as_out.state;
+	*ext_info = fc_info.as_out.ext_info;
+
+	MCDRV_DBG_VERBOSE(mcd, "exit with %d/0x%08X\n", ret, ret);
+
+	return ret;
+}
+
+/* Yield to MobiCore */
+int mc_yield(void)
+{
+	int ret = 0;
+	union fc_generic yield;
+
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
+
+	memset(&yield, 0, sizeof(yield));
+	yield.as_in.cmd = MC_SMC_N_YIELD;
+	mc_fastcall(&yield);
+	ret = convert_fc_ret(yield.as_out.ret);
+
+	return ret;
+}
+
+/* call common notify */
+int mc_nsiq(void)
+{
+	int ret = 0;
+	union fc_generic nsiq;
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
+
+	memset(&nsiq, 0, sizeof(nsiq));
+	nsiq.as_in.cmd = MC_SMC_N_SIQ;
+	mc_fastcall(&nsiq);
+	ret = convert_fc_ret(nsiq.as_out.ret);
+
+	return ret;
+}
+
+/* Call the INIT fastcall to setup MobiCore initialization */
+int mc_init(uint32_t base, uint32_t nq_offset, uint32_t nq_length,
+	uint32_t mcp_offset, uint32_t mcp_length)
+{
+	int ret = 0;
+	union mc_fc_init fc_init;
+
+	MCDRV_DBG_VERBOSE(mcd, "enter\n");
+
+	memset(&fc_init, 0, sizeof(fc_init));
+
+	fc_init.as_in.cmd = MC_FC_INIT;
+	/* base address of mci buffer 4KB aligned */
+	fc_init.as_in.base = base;
+	/* notification buffer start/length [16:16] [start, length] */
+	fc_init.as_in.nq_info = (nq_offset << 16) | (nq_length & 0xFFFF);
+	/* mcp buffer start/length [16:16] [start, length] */
+	fc_init.as_in.mcp_info = (mcp_offset << 16) | (mcp_length & 0xFFFF);
+
+	/*
+	 * Set KMOD notification queue to start of MCI
+	 * mciInfo was already set up in mmap
+	 */
+	MCDRV_DBG(mcd,
+		  "cmd=0x%08x, base=0x%08x,nq_info=0x%08x, mcp_info=0x%08x\n",
+		  fc_init.as_in.cmd, fc_init.as_in.base, fc_init.as_in.nq_info,
+		  fc_init.as_in.mcp_info);
+
+	mc_fastcall(&fc_init.as_generic);
+
+	MCDRV_DBG(mcd, "out cmd=0x%08x, ret=0x%08x\n", fc_init.as_out.resp,
+		  fc_init.as_out.ret);
+
+	ret = convert_fc_ret(fc_init.as_out.ret);
+
+	MCDRV_DBG_VERBOSE(mcd, "exit with %d/0x%08X\n", ret, ret);
+
+	return ret;
+}
+
+/* Return MobiCore driver version */
+uint32_t mc_get_version(void)
+{
+	MCDRV_DBG(mcd, "MobiCore driver version is %i.%i\n",
+		  MCDRVMODULEAPI_VERSION_MAJOR,
+		  MCDRVMODULEAPI_VERSION_MINOR);
+
+	return MC_VERSION(MCDRVMODULEAPI_VERSION_MAJOR,
+					MCDRVMODULEAPI_VERSION_MINOR);
+}
diff --git a/drivers/gud/mobicore_driver/ops.h b/drivers/gud/mobicore_driver/ops.h
new file mode 100644
index 0000000..673399f
--- /dev/null
+++ b/drivers/gud/mobicore_driver/ops.h
@@ -0,0 +1,30 @@
+/*
+ * MobiCore driver module.(interface to the secure world SWD)
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_OPS_H_
+#define _MC_OPS_H_
+
+#include <linux/workqueue.h>
+#include "fastcall.h"
+
+int mc_yield(void);
+int mc_nsiq(void);
+uint32_t mc_get_version(void);
+
+int mc_info(uint32_t ext_info_id, uint32_t *state, uint32_t *ext_info);
+int mc_init(uint32_t base, uint32_t  nq_offset, uint32_t  nq_length,
+	    uint32_t mcp_offset, uint32_t  mcp_length);
+
+static inline void mc_fastcall(void *data)
+{
+	work_on_cpu(0, _smc, data);
+}
+
+#endif /* _MC_OPS_H_ */
diff --git a/drivers/gud/mobicore_driver/platforms/msm8960_surf_std/platform.h b/drivers/gud/mobicore_driver/platforms/msm8960_surf_std/platform.h
index 070222e..9efa026 100644
--- a/drivers/gud/mobicore_driver/platforms/msm8960_surf_std/platform.h
+++ b/drivers/gud/mobicore_driver/platforms/msm8960_surf_std/platform.h
@@ -1,32 +1,23 @@
-/**
- * Header file of MobiCore Driver Kernel Module Platform
- * specific structures
- *
- * @addtogroup MobiCore_Driver_Kernel_Module
- * @{
- * Internal structures of the McDrvModule
- * @file
- *
- * Header file the MobiCore Driver Kernel Module,
+/*
+ * Header file for the MobiCore Driver Kernel Module,
  * its internal structures and defines.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
 
-#ifndef _MC_DRV_PLATFORM_H_
-#define _MC_DRV_PLATFORM_H_
+#ifndef _MC_PLATFORM_H_
+#define _MC_PLATFORM_H_
 
-/** MobiCore Interrupt for Qualcomm */
+/* MobiCore Interrupt for Qualcomm */
 #define MC_INTR_SSIQ						280
 
-/** Use SMC for fastcalls */
+/* Use SMC for fastcalls */
 #define MC_SMC_FASTCALL
 
-
 /*--------------- Implementation -------------- */
 #include <mach/scm.h>
 /* from following file */
@@ -34,17 +25,16 @@
 #define SCM_CMD_MOBICORE		1
 
 extern int scm_call(u32 svc_id, u32 cmd_id, const void *cmd_buf, size_t cmd_len,
-			void *resp_buf, size_t resp_len);
+		    void *resp_buf, size_t resp_len);
 
 static inline int smc_fastcall(void *fc_generic, size_t size)
 {
 	return scm_call(SCM_SVC_MOBICORE, SCM_CMD_MOBICORE,
-			   fc_generic, size,
-			   fc_generic, size);
+			fc_generic, size,
+			fc_generic, size);
 }
 
-/** Enable mobicore mem traces */
+/* Enable mobicore mem traces */
 #define MC_MEM_TRACES
 
-#endif /* _MC_DRV_PLATFORM_H_ */
-/** @} */
+#endif /* _MC_PLATFORM_H_ */
diff --git a/drivers/gud/mobicore_driver/pm.h b/drivers/gud/mobicore_driver/pm.h
new file mode 100644
index 0000000..067f095
--- /dev/null
+++ b/drivers/gud/mobicore_driver/pm.h
@@ -0,0 +1,17 @@
+/*
+ * Header file of MobiCore Driver Kernel Module.
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef _MC_PM_H_
+#define _MC_PM_H_
+
+/* How much time after resume the daemon should back off */
+#define DAEMON_BACKOFF_TIME	10
+
+#endif /* _MC_PM_H_ */
diff --git a/drivers/gud/mobicore_driver/public/mc_drv_module_api.h b/drivers/gud/mobicore_driver/public/mc_drv_module_api.h
deleted file mode 100644
index 59366f3..0000000
--- a/drivers/gud/mobicore_driver/public/mc_drv_module_api.h
+++ /dev/null
@@ -1,311 +0,0 @@
-/** @addtogroup MCD_MCDIMPL_KMOD_API Mobicore Driver Module API
- * @ingroup  MCD_MCDIMPL_KMOD
- * @{
- * Interface to Mobicore Driver Kernel Module.
- * @file
- *
- * <h2>Introduction</h2>
- * The MobiCore Driver Kernel Module is a Linux device driver, which represents
- * the command proxy on the lowest layer to the secure world (Swd). Additional
- * services like memory allocation via mmap and generation of a L2 tables for
- * given virtual memory are also supported. IRQ functionallity receives
- * information from the SWd in the non secure world (NWd).
- * As customary the driver is handled as linux device driver with "open",
- * "close" and "ioctl" commands. Access to the driver is possible after the
- * device "/dev/mobicore" has been opened.
- * The MobiCore Driver Kernel Module must be installed via
- * "insmod mcDrvModule.ko".
- *
- *
- * <h2>Version history</h2>
- * <table class="customtab">
- * <tr><td width="100px"><b>Date</b></td><td width="80px"><b>Version</b></td>
- * <td><b>Changes</b></td></tr>
- * <tr><td>2010-05-25</td><td>0.1</td><td>Initial Release</td></tr>
- * </table>
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- *	notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *	notice, this list of conditions and the following disclaimer in the
- *	documentation and/or other materials provided with the distribution.
- * 3. The name of the author may not be used to endorse or promote
- *	products derived from this software without specific prior
- *	written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
- * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
- * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
- * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
- * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
- * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef _MC_DRV_MODULEAPI_H_
-#define _MC_DRV_MODULEAPI_H_
-
-#include "version.h"
-
-#define MC_DRV_MOD_DEVNODE		   "mobicore"
-#define MC_DRV_MOD_DEVNODE_FULLPATH  "/dev/" MC_DRV_MOD_DEVNODE
-
-/**
- * Data exchange structure of the MC_DRV_MODULE_INIT ioctl command.
- * INIT request data to SWD
- */
-union mc_ioctl_init_params {
-	struct {
-		/** base address of mci buffer 4KB align */
-		uint32_t  base;
-		/** notification buffer start/length [16:16] [start, length] */
-		uint32_t  nq_offset;
-		/** length of notification queue */
-		uint32_t  nq_length;
-		/** mcp buffer start/length [16:16] [start, length] */
-		uint32_t  mcp_offset;
-		/** length of mcp buffer */
-		uint32_t  mcp_length;
-	} in;
-	struct {
-		/* nothing */
-	} out;
-};
-
-
-/**
- * Data exchange structure of the MC_DRV_MODULE_INFO ioctl command.
- * INFO request data to the SWD
- */
-union mc_ioctl_info_params {
-	struct {
-		uint32_t  ext_info_id; /**< extended info ID */
-	} in;
-	struct {
-		uint32_t  state; /**< state */
-		uint32_t  ext_info; /**< extended info */
-	} out;
-};
-
-/**
- * Mmap allocates and maps contiguous memory into a process.
- * We use the third parameter, void *offset, to distinguish between some cases
- * offset = MC_DRV_KMOD_MMAP_WSM	usual operation, pages are registered in
-					device structure and freed later.
- * offset = MC_DRV_KMOD_MMAP_MCI	get Instance of MCI, allocates or mmaps
-					the MCI to daemon
- * offset = MC_DRV_KMOD_MMAP_PERSISTENTWSM	special operation, without
-						registration of pages
- *
- * In mmap(), the offset specifies which of several device I/O pages is
- *  requested. Linux only transfers the page number, i.e. the upper 20 bits to
- *  kernel module. Therefore we define our special offsets as multiples of page
- *  size.
- */
-enum mc_mmap_memtype {
-	MC_DRV_KMOD_MMAP_WSM		= 0,
-	MC_DRV_KMOD_MMAP_MCI		= 4096,
-	MC_DRV_KMOD_MMAP_PERSISTENTWSM	= 8192
-};
-
-struct mc_mmap_resp {
-	uint32_t  handle; /**< WSN handle */
-	uint32_t  phys_addr; /**< physical address of WSM (or NULL) */
-	bool	  is_reused; /**< if WSM memory was reused, or new allocated */
-};
-
-/**
- * Data exchange structure of the MC_DRV_KMOD_IOCTL_FREE ioctl command.
- */
-union mc_ioctl_free_params {
-	struct {
-		uint32_t  handle; /**< driver handle */
-		uint32_t  pid; /**< process id */
-	} in;
-	struct {
-		/* nothing */
-	} out;
-};
-
-
-/**
- * Data exchange structure of the MC_DRV_KMOD_IOCTL_APP_REGISTER_WSM_L2 command.
- *
- * Allocates a physical L2 table and maps the buffer into this page.
- * Returns the physical address of the L2 table.
- * The page alignment will be created and the appropriated pSize and pOffsetL2
- * will be modified to the used values.
- */
-union mc_ioctl_app_reg_wsm_l2_params {
-	struct {
-		uint32_t  buffer; /**< base address of the virtual address  */
-		uint32_t  len; /**< size of the virtual address space */
-		uint32_t  pid; /**< process id */
-	} in;
-	struct {
-		uint32_t  handle; /**< driver handle for locked memory */
-		uint32_t  phys_wsm_l2_table; /* physical address of the L2 table */
-	} out;
-};
-
-
-/**
- * Data exchange structure of the MC_DRV_KMOD_IOCTL_APP_UNREGISTER_WSM_L2
- * command.
- */
-struct mc_ioctl_app_unreg_wsm_l2_params {
-	struct {
-		uint32_t  handle; /**< driver handle for locked memory */
-		uint32_t  pid; /**< process id */
-	} in;
-	struct {
-		/* nothing */
-	} out;
-};
-
-
-/**
- * Data exchange structure of the MC_DRV_KMOD_IOCTL_DAEMON_LOCK_WSM_L2 command.
- */
-struct mc_ioctl_daemon_lock_wsm_l2_params {
-	struct {
-		uint32_t  handle; /**< driver handle for locked memory */
-	} in;
-	struct {
-		uint32_t phys_wsm_l2_table;
-	} out;
-};
-
-
-/**
- * Data exchange structure of the MC_DRV_KMOD_IOCTL_DAEMON_UNLOCK_WSM_L2
- * command.
- */
-struct mc_ioctl_daemon_unlock_wsm_l2_params {
-	struct {
-		uint32_t  handle; /**< driver handle for locked memory */
-	} in;
-	struct {
-		/* nothing */
-	} out;
-};
-
-/**
- * Data exchange structure of the MC_DRV_MODULE_FC_EXECUTE ioctl command.
- */
-union mc_ioctl_fc_execute_params {
-	struct {
-		/**< base address of mobicore binary */
-		uint32_t  phys_start_addr;
-		/**< length of DDR area */
-		uint32_t  length;
-	} in;
-	struct {
-		/* nothing */
-	} out;
-};
-
-/**
- * Data exchange structure of the MC_DRV_MODULE_GET_VERSION ioctl command.
- */
-struct mc_ioctl_get_version_params {
-	struct {
-		uint32_t	kernel_module_version;
-	} out;
-};
-
-/* @defgroup Mobicore_Driver_Kernel_Module_Interface IOCTL */
-
-
-
-
-/* TODO: use IOCTL macros like _IOWR. See Documentation/ioctl/ioctl-number.txt,
-	Documentation/ioctl/ioctl-decoding.txt */
-/**
- * defines for the ioctl mobicore driver module function call from user space.
- */
-enum mc_kmod_ioctl {
-
-	/*
-	 * get detailed MobiCore Status
-	 */
-	MC_DRV_KMOD_IOCTL_DUMP_STATUS  = 200,
-
-	/*
-	 * initialize MobiCore
-	 */
-	MC_DRV_KMOD_IOCTL_FC_INIT  = 201,
-
-	/*
-	 * get MobiCore status
-	 */
-	MC_DRV_KMOD_IOCTL_FC_INFO  = 202,
-
-	/**
-	 * ioctl parameter to send the YIELD command to the SWD.
-	 * Only possible in Privileged Mode.
-	 * ioctl(fd, MC_DRV_MODULE_YIELD)
-	 */
-	MC_DRV_KMOD_IOCTL_FC_YIELD =  203,
-	/**
-	 * ioctl parameter to send the NSIQ signal to the SWD.
-	 * Only possible in Privileged Mode
-	 * ioctl(fd, MC_DRV_MODULE_NSIQ)
-	 */
-	MC_DRV_KMOD_IOCTL_FC_NSIQ   =  204,
-	/**
-	 * ioctl parameter to tzbsp to start Mobicore binary from DDR.
-	 * Only possible in Privileged Mode
-	 * ioctl(fd, MC_DRV_KMOD_IOCTL_FC_EXECUTE)
-	 */
-	MC_DRV_KMOD_IOCTL_FC_EXECUTE =  205,
-
-	/**
-	 * Free's memory which is formerly allocated by the driver's mmap
-	 * command. The parameter must be this mmaped address.
-	 * The internal instance data regarding to this address are deleted as
-	 * well as each according memory page and its appropriated reserved bit
-	 * is cleared (ClearPageReserved).
-	 * Usage: ioctl(fd, MC_DRV_MODULE_FREE, &address) with address beeing of
-	 * type long address
-	 */
-	MC_DRV_KMOD_IOCTL_FREE = 218,
-
-	/**
-	 * Creates a L2 Table of the given base address and the size of the
-	 * data.
-	 * Parameter: mc_ioctl_app_reg_wsm_l2_params
-	 */
-	MC_DRV_KMOD_IOCTL_APP_REGISTER_WSM_L2 = 220,
-
-	/**
-	 * Frees the L2 table created by a MC_DRV_KMOD_IOCTL_APP_REGISTER_WSM_L2
-	 * ioctl.
-	 * Parameter: mc_ioctl_app_unreg_wsm_l2_params
-	 */
-	MC_DRV_KMOD_IOCTL_APP_UNREGISTER_WSM_L2 = 221,
-
-
-	/* TODO: comment this. */
-	MC_DRV_KMOD_IOCTL_DAEMON_LOCK_WSM_L2 = 222,
-	MC_DRV_KMOD_IOCTL_DAEMON_UNLOCK_WSM_L2 = 223,
-
-	/**
-	 * Return kernel driver version.
-	 * Parameter: mc_ioctl_get_version_params
-	 */
-	MC_DRV_KMOD_IOCTL_GET_VERSION = 224,
-};
-
-
-#endif /* _MC_DRV_MODULEAPI_H_ */
-/** @} */
diff --git a/drivers/gud/mobicore_driver/public/mc_kernel_api.h b/drivers/gud/mobicore_driver/public/mc_kernel_api.h
index fdfc618..7a038c4 100644
--- a/drivers/gud/mobicore_driver/public/mc_kernel_api.h
+++ b/drivers/gud/mobicore_driver/public/mc_kernel_api.h
@@ -1,100 +1,76 @@
-/** @addtogroup MCD_MCDIMPL_KMOD_KAPI Mobicore Driver Module API inside Kernel.
- * @ingroup  MCD_MCDIMPL_KMOD
- * @{
- * Interface to Mobicore Driver Kernel Module inside Kernel.
- * @file
- *
+/*
  * Interface to be used by module MobiCoreKernelAPI.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
 
-#ifndef _MOBICORE_KERNELMODULE_API_H_
-#define _MOBICORE_KERNELMODULE_API_H_
+#ifndef _MC_KERNEL_API_H_
+#define _MC_KERNEL_API_H_
 
 struct mc_instance;
 
-/**
- * Initialize a new mobicore API instance object
+/*
+ * mobicore_open() - Initialize a new MobiCore API instance object
  *
- * @return Instance or NULL if no allocation was possible.
+ * Returns a MobiCore Instance or NULL if no allocation was possible.
  */
-struct mc_instance *mobicore_open(
-	void
-);
+struct mc_instance *mobicore_open(void);
 
-/**
- * Release a mobicore instance object and all objects related to it
- * @param instance instance
- * @return 0 if Ok or -E ERROR
+/*
+ * mobicore_release() - Release a MobiCore instance object
+ * @instance:		MobiCore instance
+ *
+ * Returns 0 if Ok or -E ERROR
  */
-int mobicore_release(
-	struct mc_instance	*instance
-);
+int mobicore_release(struct mc_instance *instance);
 
-/**
- * Free a WSM buffer allocated with mobicore_allocate_wsm
- * @param instance
- * @param handle		handle of the buffer
+/*
+ * mobicore_allocate_wsm() - Allocate MobiCore WSM
+ * @instance:		instance data for MobiCore Daemon and TLCs
+ * @requested_size:	memory size requested in bytes
+ * @handle:		pointer to handle
+ * @kernel_virt_addr:	virtual user start address
+ * @phys_addr:		physical start address
  *
- * @return 0 if no error
- *
+ * Returns 0 if OK
  */
-int mobicore_allocate_wsm(
-	struct mc_instance	*instance,
-	unsigned long		requested_size,
-	uint32_t		*handle,
-	void			**kernel_virt_addr,
-	void			**phys_addr
-);
+int mobicore_allocate_wsm(struct mc_instance *instance,
+			  unsigned long requested_size, uint32_t *handle,
+			  void **virt_kernel_addr, void **phys_addr);
 
-/**
- * Free a WSM buffer allocated with mobicore_allocate_wsm
- * @param instance
- * @param handle		handle of the buffer
+/*
+ * mobicore_free() - Free a WSM buffer allocated with mobicore_allocate_wsm
+ * @instance:		instance data for MobiCore Daemon and TLCs
+ * @handle:		handle of the buffer
  *
- * @return 0 if no error
- *
+ * Returns 0 if OK
  */
-int mobicore_free(
-	struct mc_instance	*instance,
-	uint32_t		handle
-);
+int mobicore_free_wsm(struct mc_instance *instance, uint32_t handle);
 
-/**
- * Map a virtual memory buffer structure to Mobicore
- * @param instance
- * @param addr		address of the buffer(NB it must be kernel virtual!)
- * @param len		buffer length
- * @param handle	pointer to handle
- * @param phys_wsm_l2_table	pointer to physical L2 table(?)
+/*
+ * mobicore_map_vmem() - Map a virtual memory buffer structure to Mobicore
+ * @instance:		instance data for MobiCore Daemon and TLCs
+ * @addr:		address of the buffer (NB it must be kernel virtual!)
+ * @len:		buffer length (in bytes)
+ * @handle:		unique handle
+ * @phys:		pointer for physical address of L2 table
  *
- * @return 0 if no error
- *
+ * Returns 0 if no error
  */
-int mobicore_map_vmem(
-	struct mc_instance	*instance,
-	void			*addr,
-	uint32_t		len,
-	uint32_t		*handle,
-	void			**phys_wsm_l2_table
-);
+int mobicore_map_vmem(struct mc_instance *instance, void *addr,
+		      uint32_t len, uint32_t *handle, uint32_t *phys);
 
-/**
- * Unmap a virtual memory buffer from mobicore
- * @param instance
- * @param handle
+/*
+ * mobicore_unmap_vmem() - Unmap a virtual memory buffer from MobiCore
+ * @instance:		instance data for MobiCore Daemon and TLCs
+ * @handle:		unique handle
  *
- * @return 0 if no error
- *
+ * Returns 0 if no error
  */
-int mobicore_unmap_vmem(
-	struct mc_instance	*instance,
-	uint32_t		handle
-);
-#endif /* _MOBICORE_KERNELMODULE_API_H_ */
-/** @} */
+int mobicore_unmap_vmem(struct mc_instance *instance, uint32_t handle);
+
+#endif /* _MC_KERNEL_API_H_ */
diff --git a/drivers/gud/mobicore_driver/public/mc_linux.h b/drivers/gud/mobicore_driver/public/mc_linux.h
new file mode 100644
index 0000000..99b7769
--- /dev/null
+++ b/drivers/gud/mobicore_driver/public/mc_linux.h
@@ -0,0 +1,209 @@
+/*
+ * The MobiCore Driver Kernel Module is a Linux device driver, which represents
+ * the command proxy on the lowest layer to the secure world (Swd). Additional
+ * services like memory allocation via mmap and generation of a L2 tables for
+ * given virtual memory are also supported. IRQ functionality receives
+ * information from the SWd in the non secure world (NWd).
+ * As customary the driver is handled as linux device driver with "open",
+ * "close" and "ioctl" commands. Access to the driver is possible after the
+ * device "/dev/mobicore" has been opened.
+ * The MobiCore Driver Kernel Module must be installed via
+ * "insmod mcDrvModule.ko".
+ *
+ * <-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *	notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	notice, this list of conditions and the following disclaimer in the
+ *	documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote
+ *	products derived from this software without specific prior
+ *	written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS
+ * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
+ * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MC_LINUX_H_
+#define _MC_LINUX_H_
+
+#include "version.h"
+
+#define MC_ADMIN_DEVNODE	"mobicore"
+#define MC_USER_DEVNODE		"mobicore-user"
+
+/*
+ * Data exchange structure of the MC_DRV_MODULE_INIT ioctl command.
+ * INIT request data to SWD
+ */
+struct mc_ioctl_init {
+	/* notification buffer start/length [16:16] [start, length] */
+	uint32_t  nq_offset;
+	/* length of notification queue */
+	uint32_t  nq_length;
+	/* mcp buffer start/length [16:16] [start, length] */
+	uint32_t  mcp_offset;
+	/* length of mcp buffer */
+	uint32_t  mcp_length;
+};
+
+/*
+ * Data exchange structure of the MC_DRV_MODULE_INFO ioctl command.
+ * INFO request data to the SWD
+ */
+struct mc_ioctl_info {
+	uint32_t  ext_info_id;	/* extended info ID */
+	uint32_t  state;	/* state */
+	uint32_t  ext_info;	/* extended info */
+};
+
+/*
+ * Data exchange structure of the MC_IO_MAP_WSM, MC_IO_MAP_MCI, and
+ *				  MC_IO_MAP_PWSM commands.
+ *
+ * Allocate a contiguous memory buffer for a process.
+ * The physical address can be used as for later calls to mmap.
+ * The handle can be used to communicate about this buffer to the Daemon.
+ * For MC_IO_MAP_MCI command, the reused field indicates that MCI was set up
+ * already. I.e. Daemon was restarted.
+ */
+struct mc_ioctl_map {
+	size_t	      len;	/* Buffer length */
+	uint32_t      handle;	/* WSM handle */
+	unsigned long addr;	/* Virtual address */
+	unsigned long phys_addr;/* physical address of WSM (or NULL) */
+	bool	      reused;	/* if WSM memory was reused, or new allocated */
+};
+
+/*
+ * Data exchange structure of the MC_IO_REG_WSM command.
+ *
+ * Allocates a physical L2 table and maps the buffer into this page.
+ * Returns the physical address of the L2 table.
+ * The page alignment will be created and the appropriated pSize and pOffsetL2
+ * will be modified to the used values.
+ */
+struct mc_ioctl_reg_wsm {
+	uint32_t buffer;	/* base address of the virtual address  */
+	uint32_t len;		/* size of the virtual address space */
+	uint32_t pid;		/* process id */
+	uint32_t handle;	/* driver handle for locked memory */
+	uint32_t table_phys;	/* physical address of the L2 table */
+};
+
+
+/*
+ * Data exchange structure of the MC_DRV_MODULE_FC_EXECUTE ioctl command.
+ * internal, unsupported
+ */
+struct mc_ioctl_execute {
+	/* base address of mobicore binary */
+	uint32_t phys_start_addr;
+	/* length of DDR area */
+	uint32_t length;
+};
+
+/*
+ * Data exchange structure of the MC_IO_RESOLVE_CONT_WSM ioctl command.
+ */
+struct mc_ioctl_resolv_cont_wsm {
+	/* driver handle for buffer */
+	uint32_t handle;
+	/* base address of memory */
+	uint32_t phys;
+	/* length memory */
+	uint32_t length;
+};
+
+
+/*
+ * defines for the ioctl mobicore driver module function call from user space.
+ */
+/* MobiCore IOCTL magic number */
+#define MC_IOC_MAGIC	'M'
+
+#define MC_IO_INIT		_IOWR(MC_IOC_MAGIC, 0, struct mc_ioctl_init)
+#define MC_IO_INFO		_IOWR(MC_IOC_MAGIC, 1, struct mc_ioctl_info)
+#define MC_IO_VERSION		_IOR(MC_IOC_MAGIC, 2, uint32_t)
+/*
+ * ioctl parameter to send the YIELD command to the SWD.
+ * Only possible in Privileged Mode.
+ * ioctl(fd, MC_DRV_MODULE_YIELD)
+ */
+#define MC_IO_YIELD		_IO(MC_IOC_MAGIC, 3)
+/*
+ * ioctl parameter to send the NSIQ signal to the SWD.
+ * Only possible in Privileged Mode
+ * ioctl(fd, MC_DRV_MODULE_NSIQ)
+ */
+#define MC_IO_NSIQ		_IO(MC_IOC_MAGIC, 4)
+/*
+ * Free's memory which is formerly allocated by the driver's mmap
+ * command. The parameter must be this mmaped address.
+ * The internal instance data regarding to this address are deleted as
+ * well as each according memory page and its appropriated reserved bit
+ * is cleared (ClearPageReserved).
+ * Usage: ioctl(fd, MC_DRV_MODULE_FREE, &address) with address being of
+ * type long address
+ */
+#define MC_IO_FREE		_IO(MC_IOC_MAGIC, 5)
+/*
+ * Creates a L2 Table of the given base address and the size of the
+ * data.
+ * Parameter: mc_ioctl_app_reg_wsm_l2_params
+ */
+#define MC_IO_REG_WSM		_IOWR(MC_IOC_MAGIC, 6, struct mc_ioctl_reg_wsm)
+#define MC_IO_UNREG_WSM		_IO(MC_IOC_MAGIC, 7)
+#define MC_IO_LOCK_WSM		_IO(MC_IOC_MAGIC, 8)
+#define MC_IO_UNLOCK_WSM	_IO(MC_IOC_MAGIC, 9)
+#define MC_IO_EXECUTE		_IOWR(MC_IOC_MAGIC, 10, struct mc_ioctl_execute)
+
+/*
+ * Allocate contiguous memory for a process for later mapping with mmap.
+ * MC_DRV_KMOD_MMAP_WSM	usual operation, pages are registered in
+ *					device structure and freed later.
+ * MC_DRV_KMOD_MMAP_MCI	get Instance of MCI, allocates or mmaps
+ *					the MCI to daemon
+ * MC_DRV_KMOD_MMAP_PERSISTENTWSM	special operation, without
+ *						registration of pages
+ */
+#define MC_IO_MAP_WSM		_IOWR(MC_IOC_MAGIC, 11, struct mc_ioctl_map)
+#define MC_IO_MAP_MCI		_IOWR(MC_IOC_MAGIC, 12, struct mc_ioctl_map)
+#define MC_IO_MAP_PWSM		_IOWR(MC_IOC_MAGIC, 13, struct mc_ioctl_map)
+
+/*
+ * Clean orphaned WSM buffers. Only available to the daemon and should
+ * only be carried out if the TLC crashes or otherwise calls exit() in
+ * an unexpected manner.
+ * The clean is needed together with the lock/unlock mechanism so the daemon
+ * has clear control of the mapped buffers so it can close a Trustlet before
+ * release all the WSM buffers, otherwise the Trustlet would be able to write
+ * to possibly kernel memory areas
+ */
+#define MC_IO_CLEAN_WSM		_IO(MC_IOC_MAGIC, 14)
+
+/*
+ * Get L2 phys address of a buffer handle allocated to the user.
+ * Only available to the daemon.
+ */
+#define MC_IO_RESOLVE_WSM	_IOWR(MC_IOC_MAGIC, 15, uint32_t)
+
+/*
+ * Get the phys address & length of a allocated contiguous buffer.
+ * Only available to the daemon */
+#define MC_IO_RESOLVE_CONT_WSM	_IOWR(MC_IOC_MAGIC, 16, struct mc_ioctl_execute)
+
+#endif /* _MC_LINUX_H_ */
diff --git a/drivers/gud/mobicore_driver/public/version.h b/drivers/gud/mobicore_driver/public/version.h
index 9b2dbca..b08dd95 100644
--- a/drivers/gud/mobicore_driver/public/version.h
+++ b/drivers/gud/mobicore_driver/public/version.h
@@ -1,6 +1,5 @@
-/** @addtogroup MCD_MCDIMPL_KMOD
- * @{
- * <!-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2010-2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -30,7 +29,7 @@
 #ifndef _MC_DRV_VERSION_H_
 #define _MC_DRV_VERSION_H_
 
-#define MCDRVMODULEAPI_VERSION_MAJOR 0
+#define MCDRVMODULEAPI_VERSION_MAJOR 1
 #define MCDRVMODULEAPI_VERSION_MINOR 1
 
 #endif /* _MC_DRV_VERSION_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/clientlib.c b/drivers/gud/mobicore_kernelapi/clientlib.c
index 13826f2..7038e02 100644
--- a/drivers/gud/mobicore_kernelapi/clientlib.c
+++ b/drivers/gud/mobicore_kernelapi/clientlib.c
@@ -1,7 +1,7 @@
-/**
+/*
  * MobiCore KernelApi module
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -19,16 +19,15 @@
 
 #include "public/mobicore_driver_api.h"
 #include "public/mobicore_driver_cmd.h"
+#include "include/mcinq.h"
 #include "device.h"
 #include "session.h"
 
 /* device list */
 LIST_HEAD(devices);
 
-/*----------------------------------------------------------------------------*/
-static struct mcore_device_t *resolve_device_id(
-	uint32_t device_id
-) {
+static struct mcore_device_t *resolve_device_id(uint32_t device_id)
+{
 	struct mcore_device_t *tmp;
 	struct list_head *pos;
 
@@ -41,19 +40,13 @@
 	return NULL;
 }
 
-
-/*----------------------------------------------------------------------------*/
-static void add_device(
-	struct mcore_device_t *device
-) {
+static void add_device(struct mcore_device_t *device)
+{
 	list_add_tail(&(device->list), &devices);
 }
 
-
-/*----------------------------------------------------------------------------*/
-static bool remove_device(
-	uint32_t device_id
-) {
+static bool remove_device(uint32_t device_id)
+{
 	struct mcore_device_t *tmp;
 	struct list_head *pos, *q;
 
@@ -68,22 +61,18 @@
 	return false;
 }
 
-
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_open_device(
-	uint32_t device_id
-) {
+enum mc_result mc_open_device(uint32_t device_id)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 	struct connection *dev_con = NULL;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		struct mcore_device_t *device = resolve_device_id(device_id);
 		if (device != NULL) {
-			MCDRV_DBG_ERROR("Device %d already opened", device_id);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"Device %d already opened", device_id);
 			mc_result = MC_DRV_ERR_INVALID_OPERATION;
 			break;
 		}
@@ -92,6 +81,7 @@
 		dev_con = connection_new();
 		if (!connection_connect(dev_con, MC_DAEMON_PID)) {
 			MCDRV_DBG_ERROR(
+				mc_kapi,
 				"Could not setup netlink connection to PID %u",
 				MC_DAEMON_PID);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
@@ -100,12 +90,11 @@
 
 		/* Forward device open to the daemon and read result */
 		struct mc_drv_cmd_open_device_t mc_drv_cmd_open_device = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-			/* .command_id = */ MC_DRV_CMD_OPEN_DEVICE
+			{
+				MC_DRV_CMD_OPEN_DEVICE
 			},
-		/* .payload = */ {
-		/* .device_id = */ device_id
+			{
+				device_id
 			}
 		};
 
@@ -114,8 +103,9 @@
 				&mc_drv_cmd_open_device,
 				sizeof(struct mc_drv_cmd_open_device_t));
 		if (len < 0) {
-			MCDRV_DBG_ERROR("CMD_OPEN_DEVICE writeCmd failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_DEVICE writeCmd failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
@@ -126,14 +116,16 @@
 					&rsp_header,
 					sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_OPEN_DEVICE readRsp failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_DEVICE readRsp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_OPEN_DEVICE failed, respId=%d",
-							rsp_header.response_id);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_DEVICE failed, respId=%d",
+					rsp_header.response_id);
 			switch (rsp_header.response_id) {
 			case MC_DRV_RSP_PAYLOAD_LENGTH_ERROR:
 				mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
@@ -154,8 +146,9 @@
 		device = mcore_device_create(device_id, dev_con);
 		if (!mcore_device_open(device, MC_DRV_MOD_DEVNODE_FULLPATH)) {
 			mcore_device_cleanup(device);
-			MCDRV_DBG_ERROR("could not open device file: %s",
-				MC_DRV_MOD_DEVNODE_FULLPATH);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"could not open device file: %s",
+					MC_DRV_MOD_DEVNODE_FULLPATH);
 			mc_result = MC_DRV_ERR_INVALID_DEVICE_FILE;
 			break;
 		}
@@ -167,25 +160,20 @@
 	if (mc_result != MC_DRV_OK)
 		connection_cleanup(dev_con);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_open_device);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_close_device(
-	uint32_t device_id
-) {
+enum mc_result mc_close_device(uint32_t device_id)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
-	/* Enter critical section */
 	do {
 		struct mcore_device_t *device = resolve_device_id(device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -193,15 +181,15 @@
 
 		/* Return if not all sessions have been closed */
 		if (mcore_device_has_sessions(device)) {
-			MCDRV_DBG_ERROR("cannot close with sessions pending");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"cannot close with sessions pending");
 			mc_result = MC_DRV_ERR_SESSION_PENDING;
 			break;
 		}
 
 		struct mc_drv_cmd_close_device_t mc_drv_cmd_close_device = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_CLOSE_DEVICE
+			{
+				MC_DRV_CMD_CLOSE_DEVICE
 			}
 		};
 		int len = connection_write_data(
@@ -210,8 +198,9 @@
 				sizeof(struct mc_drv_cmd_close_device_t));
 		/* ignore error, but log details */
 		if (len < 0) {
-			MCDRV_DBG_ERROR("CMD_CLOSE_DEVICE writeCmd failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_CLOSE_DEVICE writeCmd failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 		}
 
@@ -221,15 +210,17 @@
 					&rsp_header,
 					sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_CLOSE_DEVICE readResp failed "
-				" ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_CLOSE_DEVICE readResp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_CLOSE_DEVICE failed, respId=%d",
-							rsp_header.response_id);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_CLOSE_DEVICE failed, respId=%d",
+					rsp_header.response_id);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
@@ -238,44 +229,37 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_close_device);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_open_session(
-	struct mc_session_handle *session,
-	const struct mc_uuid_t	*uuid,
-	uint8_t			*tci,
-	uint32_t		len
-) {
+enum mc_result mc_open_session(struct mc_session_handle *session,
+			       const struct mc_uuid_t *uuid,
+			       uint8_t *tci, uint32_t len)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session == NULL) {
-			MCDRV_DBG_ERROR("Session is null");
+			MCDRV_DBG_ERROR(mc_kapi, "Session is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (uuid == NULL) {
-			MCDRV_DBG_ERROR("UUID is null");
+			MCDRV_DBG_ERROR(mc_kapi, "UUID is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (tci == NULL) {
-			MCDRV_DBG_ERROR("TCI is null");
+			MCDRV_DBG_ERROR(mc_kapi, "TCI is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (len > MC_MAX_TCI_LEN) {
-			MCDRV_DBG_ERROR("TCI length is longer than %d",
-				MC_MAX_TCI_LEN);
+			MCDRV_DBG_ERROR(mc_kapi, "TCI length is longer than %d",
+					MC_MAX_TCI_LEN);
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
@@ -284,7 +268,7 @@
 		struct mcore_device_t *device =
 				resolve_device_id(session->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -294,40 +278,40 @@
 		struct wsm *wsm =
 			mcore_device_find_contiguous_wsm(device, tci);
 		if (wsm == NULL) {
-			MCDRV_DBG_ERROR("Could not resolve TCI phy address ");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"Could not resolve TCI phy address ");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 
 		if (wsm->len < len) {
-			MCDRV_DBG_ERROR("length is more than allocated TCI");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"length is more than allocated TCI");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 
 		/* Prepare open session command */
 		struct mc_drv_cmd_open_session_t cmdOpenSession = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_OPEN_SESSION
+			{
+				MC_DRV_CMD_OPEN_SESSION
 			},
-			/* .payload = */ {
-				/* .device_id = */ session->device_id,
-				/* .uuid = */ *uuid,
-				/* .tci = */ (uint32_t)wsm->phys_addr,
-				/* .len = */ len
+			{
+				session->device_id,
+				*uuid,
+				(uint32_t)wsm->phys_addr,
+				len
 			}
 		};
 
 		/* Transmit command data */
-
-		int len = connection_write_data(
-						dev_con,
+		int len = connection_write_data(dev_con,
 						&cmdOpenSession,
 						sizeof(cmdOpenSession));
 		if (len != sizeof(cmdOpenSession)) {
-			MCDRV_DBG_ERROR("CMD_OPEN_SESSION writeData failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_SESSION writeData failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
@@ -336,20 +320,21 @@
 
 		/* read header first */
 		struct mc_drv_response_header_t rsp_header;
-		len = connection_read_datablock(
-					dev_con,
-					&rsp_header,
-					sizeof(rsp_header));
+		len = connection_read_datablock(dev_con,
+						&rsp_header,
+						sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_OPEN_SESSION readResp failed "
-				" ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_SESSION readResp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_OPEN_SESSION failed, respId=%d",
-							rsp_header.response_id);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_SESSION failed, respId=%d",
+					rsp_header.response_id);
 			switch (rsp_header.response_id) {
 			case MC_DRV_RSP_TRUSTLET_NOT_FOUND:
 				mc_result = MC_DRV_ERR_INVALID_DEVICE_FILE;
@@ -366,14 +351,15 @@
 
 		/* read payload */
 		struct mc_drv_rsp_open_session_payload_t
-						rsp_open_session_payload;
+					rsp_open_session_payload;
 		len = connection_read_datablock(
 					dev_con,
 					&rsp_open_session_payload,
 					sizeof(rsp_open_session_payload));
 		if (len != sizeof(rsp_open_session_payload)) {
-			MCDRV_DBG_ERROR("CMD_OPEN_SESSION readPayload failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_OPEN_SESSION readPayload fail %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
@@ -383,9 +369,10 @@
 
 		/* Set up second channel for notifications */
 		struct connection *session_connection = connection_new();
-		/*TODO: no real need to connect here? */
+
 		if (!connection_connect(session_connection, MC_DAEMON_PID)) {
 			MCDRV_DBG_ERROR(
+				mc_kapi,
 				"Could not setup netlink connection to PID %u",
 				MC_DAEMON_PID);
 			connection_cleanup(session_connection);
@@ -393,42 +380,38 @@
 			break;
 		}
 
-		/*TODO CONTINOUE HERE !!!! FIX RW RETURN HANDLING!!!! */
-
 		/* Write command to use channel for notifications */
 		struct mc_drv_cmd_nqconnect_t cmd_nqconnect = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_NQ_CONNECT
+			{
+				MC_DRV_CMD_NQ_CONNECT
 			},
-			/* .payload = */ {
-				/* .device_id =  */ session->device_id,
-				/* .session_id = */ session->session_id,
-				/* .device_session_id = */
+			{
+				session->device_id,
+				session->session_id,
 				rsp_open_session_payload.device_session_id,
-				/* .session_magic = */
-					rsp_open_session_payload.session_magic
+				rsp_open_session_payload.session_magic
 			}
 		};
 		connection_write_data(session_connection,
-			&cmd_nqconnect,
-			sizeof(cmd_nqconnect));
+				      &cmd_nqconnect,
+				      sizeof(cmd_nqconnect));
 
 		/* Read command response, header first */
-		len = connection_read_datablock(
-					session_connection,
-					&rsp_header,
-					sizeof(rsp_header));
+		len = connection_read_datablock(session_connection,
+						&rsp_header,
+						sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_NQ_CONNECT readRsp failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_NQ_CONNECT readRsp failed %d",
+					len);
 			connection_cleanup(session_connection);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_NQ_CONNECT failed, respId=%d",
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_NQ_CONNECT failed, respId=%d",
 					rsp_header.response_id);
 			connection_cleanup(session_connection);
 			mc_result = MC_DRV_ERR_NQ_FAILED;
@@ -438,84 +421,78 @@
 		/* there is no payload. */
 
 		/* Session established, new session object must be created */
-		mcore_device_create_new_session(
-			device,
-			session->session_id,
-			session_connection);
+		mcore_device_create_new_session(device,
+						session->session_id,
+						session_connection);
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_open_session);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_close_session(
-	struct mc_session_handle *session
-) {
+enum mc_result mc_close_session(struct mc_session_handle *session)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session == NULL) {
-			MCDRV_DBG_ERROR("Session is null");
+			MCDRV_DBG_ERROR(mc_kapi, "Session is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 
-		struct mcore_device_t  *device =
+		struct mcore_device_t *device =
 					resolve_device_id(session->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
-		struct connection  *dev_con = device->connection;
+		struct connection *dev_con = device->connection;
 
-		struct session  *nq_session =
-		 mcore_device_resolve_session_id(device, session->session_id);
+		struct session *nq_session =
+			mcore_device_resolve_session_id(device,
+							session->session_id);
+
 		if (nq_session == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
 
 		/* Write close session command */
 		struct mc_drv_cmd_close_session_t cmd_close_session = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_CLOSE_SESSION
+			{
+				MC_DRV_CMD_CLOSE_SESSION
 			},
-			/* .payload = */ {
-				/* .session_id = */ session->session_id,
+			{
+				session->session_id,
 			}
 		};
-		connection_write_data(
-			dev_con,
-			&cmd_close_session,
-			sizeof(cmd_close_session));
+		connection_write_data(dev_con,
+				      &cmd_close_session,
+				      sizeof(cmd_close_session));
 
 		/* Read command response */
 		struct mc_drv_response_header_t rsp_header;
-		int len = connection_read_datablock(
-						dev_con,
-						&rsp_header,
-						sizeof(rsp_header));
+		int len = connection_read_datablock(dev_con,
+						    &rsp_header,
+						    sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_CLOSE_SESSION readRsp failed "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_CLOSE_SESSION readRsp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_CLOSE_SESSION failed, respId=%d",
-							rsp_header.response_id);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_CLOSE_SESSION failed, respId=%d",
+					rsp_header.response_id);
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -525,23 +502,19 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_close_session);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_notify(
-	struct mc_session_handle   *session
-) {
+enum mc_result mc_notify(struct mc_session_handle *session)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session == NULL) {
-			MCDRV_DBG_ERROR("Session is null");
+			MCDRV_DBG_ERROR(mc_kapi, "Session is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
@@ -549,7 +522,7 @@
 		struct mcore_device_t *device =
 					resolve_device_id(session->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -558,25 +531,23 @@
 		struct session  *nqsession =
 		 mcore_device_resolve_session_id(device, session->session_id);
 		if (nqsession == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
 
 		struct mc_drv_cmd_notify_t cmd_notify = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_NOTIFY
+			{
+				MC_DRV_CMD_NOTIFY
 			},
-			/* .payload = */ {
-				/* .session_id = */ session->session_id,
+			{
+				session->session_id
 			}
 		};
 
-		connection_write_data(
-			dev_con,
-			&cmd_notify,
-			sizeof(cmd_notify));
+		connection_write_data(dev_con,
+				      &cmd_notify,
+				      sizeof(cmd_notify));
 
 		/* Daemon will not return a response */
 
@@ -586,14 +557,12 @@
 }
 EXPORT_SYMBOL(mc_notify);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_wait_notification(
-	struct mc_session_handle  *session,
-	int32_t			timeout
-) {
+enum mc_result mc_wait_notification(struct mc_session_handle *session,
+				    int32_t timeout)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session == NULL) {
@@ -601,18 +570,19 @@
 			break;
 		}
 
-		struct mcore_device_t  *device =
+		struct mcore_device_t *device =
 					resolve_device_id(session->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
 
-		struct session  *nq_session =
-		 mcore_device_resolve_session_id(device, session->session_id);
+		struct session *nq_session =
+			mcore_device_resolve_session_id(device,
+							session->session_id);
 		if (nq_session == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
@@ -624,22 +594,26 @@
 		/* Read notification queue till it's empty */
 		for (;;) {
 			struct notification notification;
-			ssize_t num_read = connection_read_data(
-				nqconnection,
-				&notification,
-				sizeof(notification),
-				timeout);
-			/* Exit on timeout in first run. Later runs have
+			ssize_t num_read =
+				connection_read_data(nqconnection,
+						     &notification,
+						     sizeof(notification),
+						     timeout);
+			/*
+			 * Exit on timeout in first run. Later runs have
 			 * timeout set to 0.
-			 * -2 means, there is no more data. */
+			 * -2 means, there is no more data.
+			 */
 			if (count == 0 && num_read == -2) {
-				MCDRV_DBG_ERROR("read timeout");
+				MCDRV_DBG_ERROR(mc_kapi, "read timeout");
 				mc_result = MC_DRV_ERR_TIMEOUT;
 				break;
 			}
-			/* After first notification the queue will be
+			/*
+			 * After first notification the queue will be
 			 * drained, Thus we set no timeout for the
-			 * following reads */
+			 * following reads
+			 */
 			timeout = 0;
 
 			if (num_read != sizeof(struct notification)) {
@@ -647,28 +621,33 @@
 					/* failure in first read, notify it */
 					mc_result = MC_DRV_ERR_NOTIFICATION;
 					MCDRV_DBG_ERROR(
+					mc_kapi,
 					"read notification failed, "
 					"%i bytes received", (int)num_read);
 					break;
 				} else {
-					/* Read of the n-th notification
-					   failed/timeout. We don't tell the
-					   caller, as we got valid notifications
-					   before. */
+					/*
+					 * Read of the n-th notification
+					 * failed/timeout. We don't tell the
+					 * caller, as we got valid notifications
+					 * before.
+					 */
 					mc_result = MC_DRV_OK;
 					break;
 				}
 			}
 
 			count++;
-			MCDRV_DBG_VERBOSE("readNq count=%d, SessionID=%d, "
-				"Payload=%d", count,
-				notification.session_id, notification.payload);
+			MCDRV_DBG_VERBOSE(mc_kapi,
+					  "count=%d, SessionID=%d, Payload=%d",
+					  count,
+					  notification.session_id,
+					  notification.payload);
 
 			if (notification.payload != 0) {
 				/* Session end point died -> store exit code */
 				session_set_error_info(nq_session,
-					notification.payload);
+						       notification.payload);
 
 				mc_result = MC_DRV_INFO_NOTIFICATION;
 				break;
@@ -681,24 +660,17 @@
 }
 EXPORT_SYMBOL(mc_wait_notification);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_malloc_wsm(
-	uint32_t	device_id,
-	uint32_t	align,
-	uint32_t	len,
-	uint8_t		**wsm,
-	uint32_t	wsm_flags
-) {
+enum mc_result mc_malloc_wsm(uint32_t device_id, uint32_t align, uint32_t len,
+			     uint8_t **wsm, uint32_t wsm_flags)
+{
 	enum mc_result mc_result = MC_DRV_ERR_UNKNOWN;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		struct mcore_device_t *device = resolve_device_id(device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -710,7 +682,7 @@
 		struct wsm *wsm_stack =
 			mcore_device_allocate_contiguous_wsm(device, len);
 		if (wsm_stack == NULL) {
-			MCDRV_DBG_ERROR("Allocation of WSM failed");
+			MCDRV_DBG_ERROR(mc_kapi, "Allocation of WSM failed");
 			mc_result = MC_DRV_ERR_NO_FREE_MEMORY;
 			break;
 		}
@@ -720,31 +692,24 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_malloc_wsm);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_free_wsm(
-	uint32_t	device_id,
-	uint8_t		*wsm
-) {
+enum mc_result mc_free_wsm(uint32_t device_id, uint8_t *wsm)
+{
 	enum mc_result mc_result = MC_DRV_ERR_UNKNOWN;
 	struct mcore_device_t *device;
 
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 
 		/* Get the device associated wit the given session */
 		device = resolve_device_id(device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
@@ -753,14 +718,15 @@
 		struct wsm *wsm_stack =
 			mcore_device_find_contiguous_wsm(device, wsm);
 		if (wsm_stack == NULL) {
-			MCDRV_DBG_ERROR("unknown address");
+			MCDRV_DBG_ERROR(mc_kapi, "unknown address");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 
 		/* Free the given virtual address */
 		if (!mcore_device_free_contiguous_wsm(device, wsm_stack)) {
-			MCDRV_DBG_ERROR("Free of virtual address failed");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"Free of virtual address failed");
 			mc_result = MC_DRV_ERR_FREE_MEMORY_FAILED;
 			break;
 		}
@@ -768,130 +734,124 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_free_wsm);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_map(
-	struct mc_session_handle	*session_handle,
-	void				*buf,
-	uint32_t			buf_len,
-	struct mc_bulk_map		*map_info
-) {
+enum mc_result mc_map(struct mc_session_handle *session_handle, void *buf,
+		      uint32_t buf_len, struct mc_bulk_map *map_info)
+{
 	enum mc_result mc_result = MC_DRV_ERR_UNKNOWN;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session_handle == NULL) {
-			MCDRV_DBG_ERROR("session_handle is null");
+			MCDRV_DBG_ERROR(mc_kapi, "session_handle is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (map_info == NULL) {
-			MCDRV_DBG_ERROR("map_info is null");
+			MCDRV_DBG_ERROR(mc_kapi, "map_info is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (buf == NULL) {
-			MCDRV_DBG_ERROR("buf is null");
+			MCDRV_DBG_ERROR(mc_kapi, "buf is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 
 		/* Determine device the session belongs to */
-		struct mcore_device_t  *device = resolve_device_id(
-						session_handle->device_id);
+		struct mcore_device_t *device =
+				resolve_device_id(session_handle->device_id);
+
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
 		struct connection *dev_con = device->connection;
 
 		/* Get session */
-		struct session  *session =
-		mcore_device_resolve_session_id(device,
-						session_handle->session_id);
+		uint32_t session_id = session_handle->session_id;
+		struct session *session =
+				mcore_device_resolve_session_id(device,
+								session_id);
 		if (session == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
 
-		/* Register mapped bulk buffer to Kernel Module and keep mapped
-		   bulk buffer in mind */
-		struct bulk_buffer_descriptor *bulk_buf = session_add_bulk_buf(
-			session, buf, buf_len);
+		/*
+		 * Register mapped bulk buffer to Kernel Module and keep mapped
+		 * bulk buffer in mind
+		 */
+		struct bulk_buffer_descriptor *bulk_buf =
+				session_add_bulk_buf(session, buf, buf_len);
 		if (bulk_buf == NULL) {
-			MCDRV_DBG_ERROR("Error mapping bulk buffer");
+			MCDRV_DBG_ERROR(mc_kapi, "Error mapping bulk buffer");
 			mc_result = MC_DRV_ERR_BULK_MAPPING;
 			break;
 		}
 
 		/* Prepare map command */
 		struct mc_drv_cmd_map_bulk_mem_t mc_drv_cmd_map_bulk_mem = {
-			/* C++ does not support C99 designated initializers */
-			/* .header = */ {
-				/* .command_id = */ MC_DRV_CMD_MAP_BULK_BUF
+			{
+				MC_DRV_CMD_MAP_BULK_BUF
 			},
-			/* .payload = */ {
-				/* .session_id = */ session->session_id,
-				/* .phys_addr_l2; = */
-					(uint32_t)bulk_buf->phys_addr_wsm_l2,
-				/* .offset_payload = */
-					(uint32_t)(bulk_buf->virt_addr) & 0xFFF,
-				/* .len_bulk_mem = */ bulk_buf->len
+			{
+				session->session_id,
+				bulk_buf->handle,
+				(uint32_t)bulk_buf->phys_addr_wsm_l2,
+				(uint32_t)(bulk_buf->virt_addr) & 0xFFF,
+				bulk_buf->len
 			}
 		};
 
 		/* Transmit map command to MobiCore device */
-		connection_write_data(
-			dev_con,
-			&mc_drv_cmd_map_bulk_mem,
-			sizeof(mc_drv_cmd_map_bulk_mem));
+		connection_write_data(dev_con,
+				      &mc_drv_cmd_map_bulk_mem,
+				      sizeof(mc_drv_cmd_map_bulk_mem));
 
 		/* Read command response */
 		struct mc_drv_response_header_t rsp_header;
-		int len = connection_read_datablock(
-						dev_con,
-						&rsp_header,
-						sizeof(rsp_header));
+		int len = connection_read_datablock(dev_con,
+						    &rsp_header,
+						    sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_MAP_BULK_BUF readRsp failed, "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_MAP_BULK_BUF readRsp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_MAP_BULK_BUF failed, respId=%d",
-							rsp_header.response_id);
-			/* REV We ignore Daemon Error code because client cannot
-			   handle it anyhow. */
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_MAP_BULK_BUF failed, respId=%d",
+					rsp_header.response_id);
+
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 
-			/* Unregister mapped bulk buffer from Kernel Module and
-			   remove mapped bulk buffer from session maintenance */
+			/*
+			 * Unregister mapped bulk buffer from Kernel Module and
+			 * remove mapped bulk buffer from session maintenance
+			 */
 			if (!session_remove_bulk_buf(session, buf)) {
 				/* Removing of bulk buffer not possible */
-				MCDRV_DBG_ERROR("Unregistering of bulk memory"
-					"from Kernel Module failed");
+				MCDRV_DBG_ERROR(mc_kapi,
+						"Unreg of bulk memory failed");
 			}
 			break;
 		}
 
 		struct mc_drv_rsp_map_bulk_mem_payload_t
 						rsp_map_bulk_mem_payload;
-		connection_read_datablock(
-			dev_con,
-			&rsp_map_bulk_mem_payload,
-			sizeof(rsp_map_bulk_mem_payload));
+		connection_read_datablock(dev_con,
+					  &rsp_map_bulk_mem_payload,
+					  sizeof(rsp_map_bulk_mem_payload));
 
 		/* Set mapping info for Trustlet */
 		map_info->secure_virt_addr =
@@ -901,37 +861,30 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_map);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_unmap(
-	struct mc_session_handle	*session_handle,
-	void				*buf,
-	struct mc_bulk_map		*map_info
-) {
+enum mc_result mc_unmap(struct mc_session_handle *session_handle, void *buf,
+			struct mc_bulk_map *map_info)
+{
 	enum mc_result mc_result = MC_DRV_ERR_UNKNOWN;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
-
-	/* Enter critical section */
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session_handle == NULL) {
-			MCDRV_DBG_ERROR("session_handle is null");
+			MCDRV_DBG_ERROR(mc_kapi, "session_handle is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (map_info == NULL) {
-			MCDRV_DBG_ERROR("map_info is null");
+			MCDRV_DBG_ERROR(mc_kapi, "map_info is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
 		if (buf == NULL) {
-			MCDRV_DBG_ERROR("buf is null");
+			MCDRV_DBG_ERROR(mc_kapi, "buf is null");
 			mc_result = MC_DRV_ERR_INVALID_PARAMETER;
 			break;
 		}
@@ -940,79 +893,83 @@
 		struct mcore_device_t  *device =
 			resolve_device_id(session_handle->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
-		struct connection  *dev_con = device->connection;
+		struct connection *dev_con = device->connection;
 
 		/* Get session */
+		uint32_t session_id = session_handle->session_id;
 		struct session  *session =
 			mcore_device_resolve_session_id(device,
-						session_handle->session_id);
+							session_id);
 		if (session == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
 
+		uint32_t handle = session_find_bulk_buf(session, buf);
+		if (handle == 0) {
+			MCDRV_DBG_ERROR(mc_kapi, "Buffer not found");
+			mc_result = MC_DRV_ERR_BULK_UNMAPPING;
+			break;
+		}
+
+
 		/* Prepare unmap command */
 		struct mc_drv_cmd_unmap_bulk_mem_t cmd_unmap_bulk_mem = {
-				/* .header = */ {
-					/* .command_id = */
-						MC_DRV_CMD_UNMAP_BULK_BUF
+				{
+					MC_DRV_CMD_UNMAP_BULK_BUF
 				},
-				/* .payload = */ {
-					/* .session_id = */ session->session_id,
-					/* .secure_virtual_adr = */
-					(uint32_t)(map_info->secure_virt_addr),
-					/* .len_bulk_mem =
-						map_info->secure_virt_len*/
+				{
+					session->session_id,
+					handle,
+					(uint32_t)(map_info->secure_virt_addr)
 				}
 			};
 
-		connection_write_data(
-			dev_con,
-			&cmd_unmap_bulk_mem,
-			sizeof(cmd_unmap_bulk_mem));
+		connection_write_data(dev_con,
+				      &cmd_unmap_bulk_mem,
+				      sizeof(cmd_unmap_bulk_mem));
 
 		/* Read command response */
 		struct mc_drv_response_header_t rsp_header;
-		int len = connection_read_datablock(
-						dev_con,
-						&rsp_header,
-						sizeof(rsp_header));
+		int len = connection_read_datablock(dev_con,
+						    &rsp_header,
+						    sizeof(rsp_header));
 		if (len != sizeof(rsp_header)) {
-			MCDRV_DBG_ERROR("CMD_UNMAP_BULK_BUF readRsp failed, "
-				"ret=%d", len);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_UNMAP_BULK_BUF readRsp failed %d",
+					len);
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		if (rsp_header.response_id != MC_DRV_RSP_OK) {
-			MCDRV_DBG_ERROR("CMD_UNMAP_BULK_BUF failed, respId=%d",
-							rsp_header.response_id);
-			/* REV We ignore Daemon Error code because client
-			   cannot handle it anyhow. */
+			MCDRV_DBG_ERROR(mc_kapi,
+					"CMD_UNMAP_BULK_BUF failed, respId=%d",
+					rsp_header.response_id);
+
 			mc_result = MC_DRV_ERR_DAEMON_UNREACHABLE;
 			break;
 		}
 
 		struct mc_drv_rsp_unmap_bulk_mem_payload_t
 						rsp_unmap_bulk_mem_payload;
-		connection_read_datablock(
-			dev_con,
-			&rsp_unmap_bulk_mem_payload,
-			sizeof(rsp_unmap_bulk_mem_payload));
+		connection_read_datablock(dev_con,
+					  &rsp_unmap_bulk_mem_payload,
+					  sizeof(rsp_unmap_bulk_mem_payload));
 
-		/* REV axh: what about check the payload? */
-
-		/* Unregister mapped bulk buffer from Kernel Module and
-		 * remove mapped bulk buffer from session maintenance */
+		/*
+		 * Unregister mapped bulk buffer from Kernel Module and
+		 * remove mapped bulk buffer from session maintenance
+		 */
 		if (!session_remove_bulk_buf(session, buf)) {
 			/* Removing of bulk buffer not possible */
-			MCDRV_DBG_ERROR("Unregistering of bulk memory from "
-							"Kernel Module failed");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"Unregistering of bulk memory failed");
 			mc_result = MC_DRV_ERR_BULK_UNMAPPING;
 			break;
 		}
@@ -1021,20 +978,16 @@
 
 	} while (false);
 
-	/* Exit critical section */
-
 	return mc_result;
 }
 EXPORT_SYMBOL(mc_unmap);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_get_session_error_code(
-	struct mc_session_handle   *session,
-	int32_t			 *last_error
-) {
+enum mc_result mc_get_session_error_code(struct mc_session_handle *session,
+					 int32_t *last_error)
+{
 	enum mc_result mc_result = MC_DRV_OK;
 
-	MCDRV_DBG_VERBOSE("===%s()===", __func__);
+	MCDRV_DBG_VERBOSE(mc_kapi, "===%s()===", __func__);
 
 	do {
 		if (session == NULL || last_error == NULL) {
@@ -1044,23 +997,24 @@
 
 		/* Get device */
 		struct mcore_device_t *device =
-					resolve_device_id(session->device_id);
+				resolve_device_id(session->device_id);
 		if (device == NULL) {
-			MCDRV_DBG_ERROR("Device not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Device not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_DEVICE;
 			break;
 		}
 
 		/* Get session */
+		uint32_t session_id = session->session_id;
 		struct session *nqsession =
-		 mcore_device_resolve_session_id(device, session->session_id);
+				mcore_device_resolve_session_id(device,
+								session_id);
 		if (nqsession == NULL) {
-			MCDRV_DBG_ERROR("Session not found");
+			MCDRV_DBG_ERROR(mc_kapi, "Session not found");
 			mc_result = MC_DRV_ERR_UNKNOWN_SESSION;
 			break;
 		}
 
-		/* get session error code from session */
 		*last_error = session_get_last_err(nqsession);
 
 	} while (false);
@@ -1069,24 +1023,17 @@
 }
 EXPORT_SYMBOL(mc_get_session_error_code);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_driver_ctrl(
-	enum mc_driver_ctrl  param,
-	uint8_t		 *data,
-	uint32_t		len
-) {
-	MCDRV_DBG_WARN("not implemented");
+enum mc_result mc_driver_ctrl(enum mc_driver_ctrl param, uint8_t *data,
+			      uint32_t len)
+{
+	MCDRV_DBG_WARN(mc_kapi, "not implemented");
 	return MC_DRV_ERR_NOT_IMPLEMENTED;
 }
 EXPORT_SYMBOL(mc_driver_ctrl);
 
-/*----------------------------------------------------------------------------*/
-enum mc_result mc_manage(
-	uint32_t  device_id,
-	uint8_t   *data,
-	uint32_t  len
-) {
-	MCDRV_DBG_WARN("not implemented");
+enum mc_result mc_manage(uint32_t device_id, uint8_t *data, uint32_t len)
+{
+	MCDRV_DBG_WARN(mc_kapi, "not implemented");
 	return MC_DRV_ERR_NOT_IMPLEMENTED;
 }
 EXPORT_SYMBOL(mc_manage);
diff --git a/drivers/gud/mobicore_kernelapi/common.h b/drivers/gud/mobicore_kernelapi/common.h
index 2a73474..a15b1d7 100644
--- a/drivers/gud/mobicore_kernelapi/common.h
+++ b/drivers/gud/mobicore_kernelapi/common.h
@@ -1,97 +1,73 @@
-/**
+/*
+ * Common data types for use by the MobiCore Kernel API Driver
  *
- * Common data types
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef COMMON_H
-#define COMMON_H
+#ifndef _MC_KAPI_COMMON_H
+#define _MC_KAPI_COMMON_H
 
 #include "connection.h"
 #include "mcinq.h"
 
-void mcapi_insert_connection(
-	struct connection *connection
-);
+void mcapi_insert_connection(struct connection *connection);
+void mcapi_remove_connection(uint32_t seq);
+unsigned int mcapi_unique_id(void);
 
-void mcapi_remove_connection(
-	uint32_t seq
-);
+#define MC_DAEMON_PID			0xFFFFFFFF
+#define MC_DRV_MOD_DEVNODE_FULLPATH	"/dev/mobicore"
 
-unsigned int mcapi_unique_id(
-	void
-);
+/* dummy function helper macro */
+#define DUMMY_FUNCTION()		do {} while (0)
 
+/* Found in main.c */
+extern struct device *mc_kapi;
 
-#define MC_DAEMON_PID 0xFFFFFFFF
-#define MC_DRV_MOD_DEVNODE_FULLPATH "/dev/mobicore"
-
-/* dummy function helper macro. */
-#define DUMMY_FUNCTION()	do {} while (0)
-
-#define MCDRV_ERROR(txt, ...) \
-	printk(KERN_ERR "mcKernelApi %s() ### ERROR: " txt, \
-		__func__, \
-		##__VA_ARGS__)
+#define MCDRV_ERROR(dev, txt, ...) \
+	dev_err(dev, "%s() ### ERROR: " txt, __func__, ##__VA_ARGS__)
 
 #if defined(DEBUG)
 
 /* #define DEBUG_VERBOSE */
 #if defined(DEBUG_VERBOSE)
-#define MCDRV_DBG_VERBOSE		  MCDRV_DBG
+#define MCDRV_DBG_VERBOSE		MCDRV_DBG
 #else
-#define MCDRV_DBG_VERBOSE(...)	 DUMMY_FUNCTION()
+#define MCDRV_DBG_VERBOSE(...)		DUMMY_FUNCTION()
 #endif
 
-#define MCDRV_DBG(txt, ...) \
-	printk(KERN_INFO "mcKernelApi %s(): " txt, \
-		__func__, \
-		##__VA_ARGS__)
+#define MCDRV_DBG(dev, txt, ...) \
+	dev_info(dev, "%s(): " txt, __func__, ##__VA_ARGS__)
 
-#define MCDRV_DBG_WARN(txt, ...) \
-	printk(KERN_WARNING "mcKernelApi %s() WARNING: " txt, \
-		__func__, \
-		##__VA_ARGS__)
+#define MCDRV_DBG_WARN(dev, txt, ...) \
+	dev_warn(dev, "%s() WARNING: " txt, __func__, ##__VA_ARGS__)
 
-#define MCDRV_DBG_ERROR(txt, ...) \
-	printk(KERN_ERR "mcKernelApi %s() ### ERROR: " txt, \
-		__func__, \
-		##__VA_ARGS__)
-
+#define MCDRV_DBG_ERROR(dev, txt, ...) \
+	dev_err(dev, "%s() ### ERROR: " txt, __func__, ##__VA_ARGS__)
 
 #define MCDRV_ASSERT(cond) \
 	do { \
 		if (unlikely(!(cond))) { \
-			panic("mcKernelApi Assertion failed: %s:%d\n", \
-				__FILE__, __LINE__); \
+			panic("mc_kernelapi Assertion failed: %s:%d\n", \
+			      __FILE__, __LINE__); \
 		} \
 	} while (0)
 
 #elif defined(NDEBUG)
 
-#define MCDRV_DBG_VERBOSE(...)	DUMMY_FUNCTION()
-#define MCDRV_DBG(...)		DUMMY_FUNCTION()
-#define MCDRV_DBG_WARN(...)	DUMMY_FUNCTION()
-#define MCDRV_DBG_ERROR(...)	DUMMY_FUNCTION()
+#define MCDRV_DBG_VERBOSE(...)		DUMMY_FUNCTION()
+#define MCDRV_DBG(...)			DUMMY_FUNCTION()
+#define MCDRV_DBG_WARN(...)		DUMMY_FUNCTION()
+#define MCDRV_DBG_ERROR(...)		DUMMY_FUNCTION()
 
-#define MCDRV_ASSERT(...)	DUMMY_FUNCTION()
+#define MCDRV_ASSERT(...)		DUMMY_FUNCTION()
 
 #else
 #error "Define DEBUG or NDEBUG"
 #endif /* [not] defined(DEBUG_MCMODULE) */
 
+#define assert(expr)			MCDRV_ASSERT(expr)
 
-#define LOG_I MCDRV_DBG_VERBOSE
-#define LOG_W MCDRV_DBG_WARN
-#define LOG_E MCDRV_DBG_ERROR
-
-
-#define assert(expr) MCDRV_ASSERT(expr)
-
-#endif /* COMMON_H */
-
-/** @} */
+#endif /* _MC_KAPI_COMMON_H */
diff --git a/drivers/gud/mobicore_kernelapi/connection.c b/drivers/gud/mobicore_kernelapi/connection.c
index 9048ae8..03288a0 100644
--- a/drivers/gud/mobicore_kernelapi/connection.c
+++ b/drivers/gud/mobicore_kernelapi/connection.c
@@ -1,10 +1,7 @@
-/** @addtogroup MCD_MCDIMPL_DAEMON_SRV
- * @{
- * @file
- *
+/*
  * Connection data.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -26,37 +23,29 @@
 /* Define the initial state of the Data Available Semaphore */
 #define SEM_NO_DATA_AVAILABLE 0
 
-/*----------------------------------------------------------------------------*/
-struct connection *connection_new(
-	void
-) {
-	struct connection *conn = kzalloc(sizeof(struct connection),
-					GFP_KERNEL);
+struct connection *connection_new(void)
+{
+	struct connection *conn;
+
+	conn = kzalloc(sizeof(*conn), GFP_KERNEL);
 	conn->sequence_magic = mcapi_unique_id();
 	mutex_init(&conn->data_lock);
-	/* No data available */
 	sema_init(&conn->data_available_sem, SEM_NO_DATA_AVAILABLE);
 
 	mcapi_insert_connection(conn);
 	return conn;
 }
 
-/*----------------------------------------------------------------------------*/
-struct connection *connection_create(
-	int	 socket_descriptor,
-	pid_t   dest
-) {
+struct connection *connection_create(int socket_descriptor, pid_t dest)
+{
 	struct connection *conn = connection_new();
 
 	conn->peer_pid = dest;
 	return conn;
 }
 
-
-/*----------------------------------------------------------------------------*/
-void connection_cleanup(
-	struct connection *conn
-) {
+void connection_cleanup(struct connection *conn)
+{
 	if (!conn)
 		return;
 
@@ -66,26 +55,20 @@
 	kfree(conn);
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool connection_connect(
-	struct connection *conn,
-	pid_t		dest
-) {
+bool connection_connect(struct connection *conn, pid_t dest)
+{
 	/* Nothing to connect */
 	conn->peer_pid = dest;
 	return true;
 }
 
-/*----------------------------------------------------------------------------*/
-size_t connection_readDataMsg(
-	struct connection *conn,
-	void *buffer,
-	uint32_t len
-) {
+size_t connection_read_data_msg(struct connection *conn, void *buffer,
+				uint32_t len)
+{
 	size_t ret = -1;
-	MCDRV_DBG_VERBOSE("reading connection data %u, connection data left %u",
-			len, conn->data_len);
+	MCDRV_DBG_VERBOSE(mc_kapi,
+			  "reading connection data %u, connection data left %u",
+			  len, conn->data_len);
 	/* trying to read more than the left data */
 	if (len > conn->data_len) {
 		ret = conn->data_len;
@@ -98,80 +81,75 @@
 		conn->data_start += len;
 	}
 
-	if (conn->data_len == 0)	{
+	if (conn->data_len == 0) {
 		conn->data_start = NULL;
 		kfree_skb(conn->skb);
 		conn->skb = NULL;
 	}
-	MCDRV_DBG_VERBOSE("read %u",  ret);
+	MCDRV_DBG_VERBOSE(mc_kapi, "read %u",  ret);
 	return ret;
 }
 
-/*----------------------------------------------------------------------------*/
-size_t connection_read_datablock(
-	struct connection *conn,
-	void		 *buffer,
-	uint32_t	 len
-) {
+size_t connection_read_datablock(struct connection *conn, void *buffer,
+				 uint32_t len)
+{
 	return connection_read_data(conn, buffer, len, -1);
 }
 
-
-/*----------------------------------------------------------------------------*/
-size_t connection_read_data(
-	struct connection *conn,
-	void		*buffer,
-	uint32_t	len,
-	int32_t		timeout
-) {
+size_t connection_read_data(struct connection *conn, void *buffer, uint32_t len,
+			    int32_t timeout)
+{
 	size_t ret = 0;
 
 	MCDRV_ASSERT(buffer != NULL);
 	MCDRV_ASSERT(conn->socket_descriptor != NULL);
 
-	MCDRV_DBG_VERBOSE("read data len = %u for PID = %u",
-						len, conn->sequence_magic);
+	MCDRV_DBG_VERBOSE(mc_kapi, "read data len = %u for PID = %u",
+			  len, conn->sequence_magic);
 	do {
-		/* Wait until data is available or timeout
-		   msecs_to_jiffies(-1) -> wait forever for the sem */
+		/*
+		 * Wait until data is available or timeout
+		 * msecs_to_jiffies(-1) -> wait forever for the sem
+		 */
 		if (down_timeout(&(conn->data_available_sem),
-				  msecs_to_jiffies(timeout))) {
-			MCDRV_DBG_VERBOSE("Timeout reading the data sem");
+				 msecs_to_jiffies(timeout))) {
+			MCDRV_DBG_VERBOSE(mc_kapi,
+					  "Timeout reading the data sem");
 			ret = -2;
 			break;
 		}
 
 		if (mutex_lock_interruptible(&(conn->data_lock))) {
-			MCDRV_DBG_ERROR("interrupted reading the data sem");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"interrupted reading the data sem");
 			ret = -1;
 			break;
 		}
+
 		/* Have data, use it */
 		if (conn->data_len > 0)
-			ret = connection_readDataMsg(conn, buffer, len);
+			ret = connection_read_data_msg(conn, buffer, len);
 
-			mutex_unlock(&(conn->data_lock));
+		mutex_unlock(&(conn->data_lock));
 
 		/* There is still some data left */
 		if (conn->data_len > 0)
 			up(&conn->data_available_sem);
+
 	} while (0);
 
 	return ret;
 }
 
-/*----------------------------------------------------------------------------*/
-size_t connection_write_data(
-	struct connection *conn,
-	void		 *buffer,
-	uint32_t	 len
-) {
+size_t connection_write_data(struct connection *conn, void *buffer,
+			     uint32_t len)
+{
 	struct sk_buff *skb = NULL;
 	struct nlmsghdr *nlh;
 	int ret = 0;
 
-	MCDRV_DBG_VERBOSE("buffer length %u from pid %u\n",
-		  len,  conn->sequence_magic);
+	MCDRV_DBG_VERBOSE(mc_kapi, "buffer length %u from pid %u\n",
+			  len,  conn->sequence_magic);
 	do {
 		skb = nlmsg_new(NLMSG_SPACE(len), GFP_KERNEL);
 		if (!skb) {
@@ -180,7 +158,7 @@
 		}
 
 		nlh = nlmsg_put(skb, 0, conn->sequence_magic, 2,
-					  NLMSG_LENGTH(len), NLM_F_REQUEST);
+				NLMSG_LENGTH(len), NLM_F_REQUEST);
 		if (!nlh) {
 			ret = -1;
 			break;
@@ -188,7 +166,7 @@
 		memcpy(NLMSG_DATA(nlh), buffer, len);
 
 		netlink_unicast(conn->socket_descriptor, skb,
-						conn->peer_pid, MSG_DONTWAIT);
+				conn->peer_pid, MSG_DONTWAIT);
 		ret = len;
 	} while (0);
 
@@ -198,22 +176,20 @@
 	return ret;
 }
 
-int connection_process(
-	struct connection *conn,
-	struct sk_buff *skb
-)
+int connection_process(struct connection *conn, struct sk_buff *skb)
 {
 	int ret = 0;
 	do {
 		if (mutex_lock_interruptible(&(conn->data_lock))) {
-			MCDRV_DBG_ERROR("Interrupted getting data semaphore!");
+			MCDRV_DBG_ERROR(mc_kapi,
+					"Interrupted getting data semaphore!");
 			ret = -1;
 			break;
 		}
 
 		kfree_skb(conn->skb);
 
-		/* Get a reference to the incomming skb */
+		/* Get a reference to the incoming skb */
 		conn->skb = skb_get(skb);
 		if (conn->skb) {
 			conn->data_msg = nlmsg_hdr(conn->skb);
@@ -226,4 +202,3 @@
 	} while (0);
 	return ret;
 }
-/** @} */
diff --git a/drivers/gud/mobicore_kernelapi/connection.h b/drivers/gud/mobicore_kernelapi/connection.h
index 0b468e6..6c3ff00 100644
--- a/drivers/gud/mobicore_kernelapi/connection.h
+++ b/drivers/gud/mobicore_kernelapi/connection.h
@@ -1,122 +1,58 @@
-/** @addtogroup MCD_MCDIMPL_DAEMON_SRV
- * @{
- * @file
- *
+/*
  * Connection data.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef CONNECTION_H_
-#define CONNECTION_H_
+#ifndef _MC_KAPI_CONNECTION_H_
+#define _MC_KAPI_CONNECTION_H_
 
 #include <linux/semaphore.h>
+#include <linux/mutex.h>
 
 #include <stddef.h>
 #include <stdbool.h>
 
-#define MAX_PAYLOAD_SIZE 128
-
 struct connection {
-	struct sock *socket_descriptor; /**< Netlink socket */
-	uint32_t sequence_magic; /**< Random? magic to match requests/answers */
+	/* Netlink socket */
+	struct sock		*socket_descriptor;
+	/* Random? magic to match requests/answers */
+	uint32_t		sequence_magic;
 
-	struct nlmsghdr *data_msg;
-	uint32_t data_len; /**< How much connection data is left */
-	void *data_start; /**< Start pointer of remaining data */
-	struct sk_buff *skb;
+	struct nlmsghdr		*data_msg;
+	/* How much connection data is left */
+	uint32_t		data_len;
+	/* Start pointer of remaining data */
+	void			*data_start;
+	struct sk_buff		*skb;
 
-	struct mutex data_lock; /**< Data protection lock */
-	struct semaphore data_available_sem; /**< Data protection semaphore */
+	/* Data protection lock */
+	struct mutex		data_lock;
+	/* Data protection semaphore */
+	struct semaphore	data_available_sem;
 
-	pid_t self_pid; /**< PID address used for local connection */
-	pid_t peer_pid; /**< Remote PID for connection */
+	/* PID address used for local connection */
+	pid_t			self_pid;
+	/* Remote PID for connection */
+	pid_t			peer_pid;
 
-	struct list_head list; /**< The list param for using the kernel lists*/
+	/* The list param for using the kernel lists */
+	struct list_head	list;
 };
 
-struct connection *connection_new(
-	void
-);
+struct connection *connection_new(void);
+struct connection *connection_create(int socket_descriptor, pid_t dest);
+void connection_cleanup(struct connection *conn);
+bool connection_connect(struct connection *conn, pid_t dest);
+size_t connection_read_datablock(struct connection *conn, void *buffer,
+					uint32_t len);
+size_t connection_read_data(struct connection *conn, void *buffer,
+				   uint32_t len, int32_t timeout);
+size_t connection_write_data(struct connection *conn, void *buffer,
+				    uint32_t len);
+int connection_process(struct connection *conn, struct sk_buff *skb);
 
-struct connection *connection_create(
-	int		  socket_descriptor,
-	pid_t		dest
-);
-
-void connection_cleanup(
-	struct connection *conn
-);
-
-/**
-  * Connect to destination.
-  *
-  * @param Destination pointer.
-  * @return true on success.
-  */
-bool connection_connect(
-	struct connection *conn,
-	pid_t		dest
-);
-
-
-/**
-  * Read bytes from the connection.
-  *
-  * @param buffer	Pointer to destination buffer.
-  * @param len	   Number of bytes to read.
-  * @return Number of bytes read.
-  */
-size_t connection_read_datablock(
-	struct connection *conn,
-	void		 *buffer,
-	uint32_t	 len
-);
-/**
-  * Read bytes from the connection.
-  *
-  * @param buffer	Pointer to destination buffer.
-  * @param len	   Number of bytes to read.
-  * @param timeout   Timeout in milliseconds
-  * @return Number of bytes read.
-  * @return -1 if select() failed (returned -1)
-  * @return -2 if no data available, i.e. timeout
-  */
-size_t connection_read_data(
-	struct connection *conn,
-	void		 *buffer,
-	uint32_t	 len,
-	int32_t	  timeout
-);
-
-/**
-  * Write bytes to the connection.
-  *
-  * @param buffer	Pointer to source buffer.
-  * @param len		Number of bytes to read.
-  * @return Number of bytes written.
-  */
-size_t connection_write_data(
-	struct connection *conn,
-	void		 *buffer,
-	uint32_t	  len
-);
-
-/**
- * Write bytes to the connection.
- *
- * @param buffer	Pointer to source buffer.
- * @param len		Number of bytes to read.
- * @return Number of bytes written.
- */
-int connection_process(
-	struct connection *conn,
-	struct sk_buff *skb
-);
-
-#endif /* CONNECTION_H_ */
-
-/** @} */
+#endif /* _MC_KAPI_CONNECTION_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/device.c b/drivers/gud/mobicore_kernelapi/device.c
index dbeee6a..a176322 100644
--- a/drivers/gud/mobicore_kernelapi/device.c
+++ b/drivers/gud/mobicore_kernelapi/device.c
@@ -1,12 +1,9 @@
-/** @addtogroup MCD_IMPL_LIB
- * @{
- * @file
+/*
+ * MobiCore client library device management.
  *
- * Client library device management.
+ * Device and Trustlet Session management Functions.
  *
- * Device and Trustlet Session management Funtions.
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -14,21 +11,19 @@
  */
 #include <linux/list.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 #include "mc_kernel_api.h"
 #include "public/mobicore_driver_api.h"
 
 #include "device.h"
 #include "common.h"
 
-/*----------------------------------------------------------------------------*/
-struct wsm *wsm_create(
-	void	*virt_addr,
-	uint32_t  len,
-	uint32_t  handle,
-	void	*phys_addr /*= NULL this may be unknown, so is can be omitted.*/
-	)
+struct wsm *wsm_create(void *virt_addr, uint32_t len, uint32_t handle,
+		       void *phys_addr)
 {
-	struct wsm *wsm = kzalloc(sizeof(struct wsm), GFP_KERNEL);
+	struct wsm *wsm;
+
+	wsm = kzalloc(sizeof(*wsm), GFP_KERNEL);
 	wsm->virt_addr = virt_addr;
 	wsm->len = len;
 	wsm->handle = handle;
@@ -36,14 +31,12 @@
 	return wsm;
 }
 
+struct mcore_device_t *mcore_device_create(uint32_t device_id,
+					   struct connection *connection)
+{
+	struct mcore_device_t *dev;
 
-/*----------------------------------------------------------------------------*/
-struct mcore_device_t *mcore_device_create(
-	uint32_t  device_id,
-	struct connection  *connection
-) {
-	struct mcore_device_t *dev =
-			kzalloc(sizeof(struct mcore_device_t), GFP_KERNEL);
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	dev->device_id = device_id;
 	dev->connection = connection;
 
@@ -53,17 +46,16 @@
 	return dev;
 }
 
-
-/*----------------------------------------------------------------------------*/
-void mcore_device_cleanup(
-	struct mcore_device_t *dev
-) {
+void mcore_device_cleanup(struct mcore_device_t *dev)
+{
 	struct session *tmp;
 	struct wsm *wsm;
 	struct list_head *pos, *q;
 
-	/* Delete all session objects. Usually this should not be needed
-	 * as closeDevice()requires that all sessions have been closed before.*/
+	/*
+	 * Delete all session objects. Usually this should not be needed
+	 * as close_device() requires that all sessions have been closed before.
+	 */
 	list_for_each_safe(pos, q, &dev->session_vector) {
 		tmp = list_entry(pos, struct session, list);
 		list_del(pos);
@@ -73,7 +65,6 @@
 	/* Free all allocated WSM descriptors */
 	list_for_each_safe(pos, q, &dev->wsm_l2_vector) {
 		wsm = list_entry(pos, struct wsm, list);
-		/* mcKMod_free(dev->instance, wsm->handle); */
 		list_del(pos);
 		kfree(wsm);
 	}
@@ -83,56 +74,41 @@
 	kfree(dev);
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool mcore_device_open(
-	struct mcore_device_t   *dev,
-	const char *deviceName
-) {
+bool mcore_device_open(struct mcore_device_t *dev, const char *deviceName)
+{
 	dev->instance = mobicore_open();
 	return (dev->instance != NULL);
 }
 
-
-/*----------------------------------------------------------------------------*/
-void mcore_device_close(
-	struct mcore_device_t *dev
-) {
+void mcore_device_close(struct mcore_device_t *dev)
+{
 	mobicore_release(dev->instance);
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool mcore_device_has_sessions(
-	struct mcore_device_t *dev
-) {
+bool mcore_device_has_sessions(struct mcore_device_t *dev)
+{
 	return !list_empty(&dev->session_vector);
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool mcore_device_create_new_session(
-	struct mcore_device_t	*dev,
-	uint32_t		session_id,
-	struct connection	*connection
-) {
+bool mcore_device_create_new_session(struct mcore_device_t *dev,
+				     uint32_t session_id,
+				     struct connection *connection)
+{
 	/* Check if session_id already exists */
 	if (mcore_device_resolve_session_id(dev, session_id)) {
-		MCDRV_DBG_ERROR(" session %u already exists", session_id);
+		MCDRV_DBG_ERROR(mc_kapi,
+				" session %u already exists", session_id);
 		return false;
 	}
-	struct session *session = session_create(session_id, dev->instance,
-						connection);
+	struct session *session =
+			session_create(session_id, dev->instance, connection);
 	list_add_tail(&(session->list), &(dev->session_vector));
 	return true;
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool mcore_device_remove_session(
-	struct mcore_device_t *dev,
-	uint32_t session_id
-) {
+bool mcore_device_remove_session(struct mcore_device_t *dev,
+				 uint32_t session_id)
+{
 	bool ret = false;
 	struct session *tmp;
 	struct list_head *pos, *q;
@@ -149,17 +125,13 @@
 	return ret;
 }
 
-
-/*----------------------------------------------------------------------------*/
-struct session *mcore_device_resolve_session_id(
-	struct mcore_device_t *dev,
-	uint32_t session_id
-) {
-	struct session  *ret = NULL;
+struct session *mcore_device_resolve_session_id(struct mcore_device_t *dev,
+						uint32_t session_id)
+{
+	struct session *ret = NULL;
 	struct session *tmp;
 	struct list_head *pos;
 
-
 	/* Get session for session_id */
 	list_for_each(pos, &dev->session_vector) {
 		tmp = list_entry(pos, struct session, list);
@@ -171,26 +143,20 @@
 	return ret;
 }
 
-
-/*----------------------------------------------------------------------------*/
-struct wsm *mcore_device_allocate_contiguous_wsm(
-	struct mcore_device_t *dev,
-	uint32_t len
-) {
+struct wsm *mcore_device_allocate_contiguous_wsm(struct mcore_device_t *dev,
+						 uint32_t len)
+{
 	struct wsm *wsm = NULL;
 	do {
 		if (len == 0)
 			break;
 
 		/* Allocate shared memory */
-		void	*virt_addr;
-		uint32_t  handle;
-		void	*phys_addr;
-		int ret = mobicore_allocate_wsm(dev->instance,
-						len,
-						&handle,
-						&virt_addr,
-						&phys_addr);
+		void *virt_addr;
+		uint32_t handle;
+		void *phys_addr;
+		int ret = mobicore_allocate_wsm(dev->instance, len, &handle,
+						&virt_addr, &phys_addr);
 		if (ret != 0)
 			break;
 
@@ -201,16 +167,12 @@
 
 	} while (0);
 
-	/* Return pointer to the allocated memory */
 	return wsm;
 }
 
-
-/*----------------------------------------------------------------------------*/
-bool mcore_device_free_contiguous_wsm(
-	struct mcore_device_t  *dev,
-	struct wsm   *wsm
-) {
+bool mcore_device_free_contiguous_wsm(struct mcore_device_t *dev,
+				      struct wsm *wsm)
+{
 	bool ret = false;
 	struct wsm *tmp;
 	struct list_head *pos;
@@ -224,11 +186,12 @@
 	}
 
 	if (ret) {
-		MCDRV_DBG_VERBOSE("freeWsm virt_addr=0x%p, handle=%d",
-				wsm->virt_addr, wsm->handle);
+		MCDRV_DBG_VERBOSE(mc_kapi,
+				  "freeWsm virt_addr=0x%p, handle=%d",
+				  wsm->virt_addr, wsm->handle);
 
 		/* ignore return code */
-		mobicore_free(dev->instance, wsm->handle);
+		mobicore_free_wsm(dev->instance, wsm->handle);
 
 		list_del(pos);
 		kfree(wsm);
@@ -236,12 +199,9 @@
 	return ret;
 }
 
-
-/*----------------------------------------------------------------------------*/
-struct wsm *mcore_device_find_contiguous_wsm(
-	struct mcore_device_t *dev,
-	void   *virt_addr
-) {
+struct wsm *mcore_device_find_contiguous_wsm(struct mcore_device_t *dev,
+					     void *virt_addr)
+{
 	struct wsm *wsm;
 	struct list_head *pos;
 
@@ -253,5 +213,3 @@
 
 	return NULL;
 }
-
-/** @} */
diff --git a/drivers/gud/mobicore_kernelapi/device.h b/drivers/gud/mobicore_kernelapi/device.h
index f40d993..16626bd 100644
--- a/drivers/gud/mobicore_kernelapi/device.h
+++ b/drivers/gud/mobicore_kernelapi/device.h
@@ -1,19 +1,16 @@
-/** @addtogroup MCD_IMPL_LIB
- * @{
- * @file
- *
- * Client library device management.
+/*
+ * MobiCore client library device management.
  *
  * Device and Trustlet Session management Functions.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef DEVICE_H_
-#define DEVICE_H_
+#ifndef _MC_KAPI_DEVICE_H_
+#define _MC_KAPI_DEVICE_H_
 
 #include <linux/list.h>
 
@@ -21,119 +18,39 @@
 #include "session.h"
 #include "wsm.h"
 
-
 struct mcore_device_t {
-	struct list_head session_vector; /**< MobiCore Trustlet session
-				associated with the device */
-	struct list_head	 wsm_l2_vector; /**< WSM L2 Table  */
+	/* MobiCore Trustlet session associated with the device */
+	struct list_head	session_vector;
+	struct list_head	 wsm_l2_vector; /* WSM L2 Table  */
 
-	uint32_t		device_id; /**< Device identifier */
-	struct connection	*connection; /**< The device connection */
-	struct mc_instance  *instance; /**< MobiCore Driver instance */
+	uint32_t		device_id;	/* Device identifier */
+	struct connection	*connection;	/* The device connection */
+	struct mc_instance	*instance;	/* MobiCore Driver instance */
 
-	struct list_head list; /**< The list param for using the kernel lists*/
+	/* The list param for using the kernel lists */
+	struct list_head	list;
 };
 
 struct mcore_device_t *mcore_device_create(
-	uint32_t	  device_id,
-	struct connection  *connection
-);
+		uint32_t device_id, struct connection *connection);
+void mcore_device_cleanup(struct mcore_device_t *dev);
 
-void mcore_device_cleanup(
-	struct mcore_device_t *dev
-);
 
-/**
-  * Open the device.
-  * @param deviceName Name of the kernel modules device file.
-  * @return true if the device has been opened successfully
-  */
-bool mcore_device_open(
-	struct mcore_device_t   *dev,
-	const char *deviceName
-);
-
-/**
-  * Closes the device.
-  */
-void mcore_device_close(
-	struct mcore_device_t *dev
-);
-
-/**
-  * Check if the device has open sessions.
-  * @return true if the device has one or more open sessions.
-  */
-bool mcore_device_has_sessions(
-	struct mcore_device_t *dev
-);
-
-/**
-  * Add a session to the device.
-  * @param session_id session ID
-  * @param connection session connection
-  */
+bool mcore_device_open(struct mcore_device_t *dev, const char *deviceName);
+void mcore_device_close(struct mcore_device_t *dev);
+bool mcore_device_has_sessions(struct mcore_device_t *dev);
 bool mcore_device_create_new_session(
-	struct mcore_device_t	  *dev,
-	uint32_t	session_id,
-	struct connection  *connection
-);
-
-/**
-  * Remove the specified session from the device.
-  * The session object will be destroyed and all resources associated with it
-  * will be freed.
-  *
-  * @param session_id Session of the session to remove.
-  * @return true if a session has been found and removed.
-  */
+		struct mcore_device_t *dev, uint32_t session_id,
+		struct connection *connection);
 bool mcore_device_remove_session(
-	struct mcore_device_t *dev,
-	uint32_t session_id
-);
-
-/**
-  * Get as session object for a given session ID.
-  * @param session_id Identified of a previously opened session.
-  * @return Session object if available or NULL if no session has been found.
-  */
+		struct mcore_device_t *dev, uint32_t session_id);
 struct session *mcore_device_resolve_session_id(
-	struct mcore_device_t *dev,
-	uint32_t session_id
-);
-
-/**
-  * Allocate a block of contiguous WSM.
-  * @param len The virtual address to be registered.
-  * @return The virtual address of the allocated memory or NULL if no memory
-  * is available.
-  */
+		struct mcore_device_t *dev, uint32_t session_id);
 struct wsm *mcore_device_allocate_contiguous_wsm(
-	struct mcore_device_t *dev,
-	uint32_t len
-);
-
-/**
-  * Unregister a vaddr from a device.
-  * @param vaddr The virtual address to be registered.
-  * @param paddr The physical address to be registered.
-  */
+		struct mcore_device_t *dev, uint32_t len);
 bool mcore_device_free_contiguous_wsm(
-	struct mcore_device_t  *dev,
-	struct wsm *wsm
-);
-
-/**
-  * Get a WSM object for a given virtual address.
-  * @param vaddr The virtual address which has been allocate with mc_malloc_wsm()
-  * in advance.
-  * @return the WSM object or NULL if no address has been found.
-  */
+		struct mcore_device_t *dev, struct wsm *wsm);
 struct wsm *mcore_device_find_contiguous_wsm(
-	struct mcore_device_t *dev,
-	void   *virt_addr
-);
+		struct mcore_device_t *dev, void *virt_addr);
 
-#endif /* DEVICE_H_ */
-
-/** @} */
+#endif /* _MC_KAPI_DEVICE_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/include/mcinq.h b/drivers/gud/mobicore_kernelapi/include/mcinq.h
index 3cb82be..b874925 100644
--- a/drivers/gud/mobicore_kernelapi/include/mcinq.h
+++ b/drivers/gud/mobicore_kernelapi/include/mcinq.h
@@ -1,8 +1,8 @@
-/** @addtogroup NQ
- * @{
+/*
  * Notifications inform the MobiCore runtime environment that information is
  * pending in a WSM buffer.
- * The Trustlet Connector (TLC) and the corresponding trustlet also utilize
+ *
+ * The Trustlet Connector (TLC) and the corresponding Trustlet also utilize
  * this buffer to notify each other about new data within the
  * Trustlet Connector Interface (TCI).
  *
@@ -13,13 +13,12 @@
  *
  * Notifications hold the session ID, which is used to reference the
  * communication partner in the other world.
- * So if, e.g., the TLC in the normal world wants to notify his trustlet
+ * So if, e.g., the TLC in the normal world wants to notify his Trustlet
  * about new data in the TLC buffer
  *
- * @file
  * Notification queue declarations.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -45,81 +44,74 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef NQ_H_
-#define NQ_H_
+#ifndef _MCINQ_H_
+#define _MCINQ_H_
 
-/** \name NQ Size Defines
- * Minimum and maximum count of elements in the notification queue.
- * @{ */
-#define MIN_NQ_ELEM 1   /**< Minimum notification queue elements. */
-#define MAX_NQ_ELEM 64 /**< Maximum notification queue elements. */
-/** @} */
+/* Minimum and maximum count of elements in the notification queue */
+#define MIN_NQ_ELEM	1	/* Minimum notification queue elements. */
+#define MAX_NQ_ELEM	64	/* Maximum notification queue elements. */
 
-/** \name NQ Length Defines
- * Minimum and maximum notification queue length.
- * @{ */
-/**< Minimum notification length (in bytes). */
-#define MIN_NQ_LEN (MIN_NQ_ELEM * sizeof(notification))
-/**< Maximum notification length (in bytes). */
-#define MAX_NQ_LEN (MAX_NQ_ELEM * sizeof(notification))
-/** @} */
+/* Minimum notification length (in bytes). */
+#define MIN_NQ_LEN	(MIN_NQ_ELEM * sizeof(notification))
 
-/** \name Session ID Defines
- * Standard Session IDs.
- * @{ */
-/**< MCP session ID is used when directly communicating with the MobiCore
- * (e.g. for starting and stopping of trustlets). */
-#define SID_MCP	   0
-/**< Invalid session id is returned in case of an error. */
-#define SID_INVALID   0xffffffff
-/** @} */
+/* Maximum notification length (in bytes). */
+#define MAX_NQ_LEN	(MAX_NQ_ELEM * sizeof(notification))
 
-/** Notification data structure. */
+/*
+ * MCP session ID is used when directly communicating with the MobiCore
+ * (e.g. for starting and stopping of Trustlets).
+ */
+#define SID_MCP		0
+/* Invalid session id is returned in case of an error. */
+#define SID_INVALID	0xffffffff
+
+/* Notification data structure. */
 struct notification {
-	uint32_t session_id; /**< Session ID. */
-	int32_t payload;	/**< Additional notification information. */
+	uint32_t	session_id;	/* Session ID. */
+	int32_t		payload;	/* Additional notification info */
 };
 
-/** Notification payload codes.
+/*
+ * Notification payload codes.
  * 0 indicated a plain simple notification,
  * a positive value is a termination reason from the task,
  * a negative value is a termination reason from MobiCore.
  * Possible negative values are given below.
  */
 enum notification_payload {
-	/**< task terminated, but exit code is invalid */
-	ERR_INVALID_EXIT_CODE   = -1,
-	/**< task terminated due to session end, no exit code available */
-	ERR_SESSION_CLOSE	   = -2,
-	/**< task terminated due to invalid operation */
-	ERR_INVALID_OPERATION   = -3,
-	/**< session ID is unknown */
-	ERR_INVALID_SID		 = -4,
-	/**<  session is not active */
-	ERR_SID_NOT_ACTIVE	  = -5
+	/* task terminated, but exit code is invalid */
+	ERR_INVALID_EXIT_CODE	= -1,
+	/* task terminated due to session end, no exit code available */
+	ERR_SESSION_CLOSE	= -2,
+	/* task terminated due to invalid operation */
+	ERR_INVALID_OPERATION	= -3,
+	/* session ID is unknown */
+	ERR_INVALID_SID		= -4,
+	/*  session is not active */
+	ERR_SID_NOT_ACTIVE	= -5
 };
 
-/** Declaration of the notification queue header.
- * layout as specified in the data structure specification.
+/*
+ * Declaration of the notification queue header.
+ * Layout as specified in the data structure specification.
  */
 struct notification_queue_header {
-	uint32_t write_cnt;  /**< Write counter. */
-	uint32_t read_cnt;   /**< Read counter. */
-	uint32_t queue_size; /**< Queue size. */
+	uint32_t	write_cnt;	/* Write counter. */
+	uint32_t	read_cnt;	/* Read counter. */
+	uint32_t	queue_size;	/* Queue size. */
 };
 
-/** Queue struct which defines a queue object.
+/*
+ * Queue struct which defines a queue object.
  * The queue struct is accessed by the queue<operation> type of
  * function. elementCnt must be a power of two and the power needs
  * to be smaller than power of uint32_t (obviously 32).
  */
 struct notification_queue {
-	/**< Queue header. */
+	/* Queue header. */
 	struct notification_queue_header hdr;
-	/**< Notification elements. */
+	/* Notification elements. */
 	struct notification notification[MIN_NQ_ELEM];
 } ;
 
-#endif /** NQ_H_ */
-
-/** @} */
+#endif /* _MCINQ_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/include/mcuuid.h b/drivers/gud/mobicore_kernelapi/include/mcuuid.h
index b72acb8..6da9437 100644
--- a/drivers/gud/mobicore_kernelapi/include/mcuuid.h
+++ b/drivers/gud/mobicore_kernelapi/include/mcuuid.h
@@ -1,7 +1,5 @@
-/**
- * @addtogroup MC_UUID mcUuid - Universally Unique Identifier.
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2011-2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2011-2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -27,21 +25,19 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
- * @ingroup  MC_DATA_TYPES
- * @{
  */
 
-#ifndef MC_UUID_H_
-#define MC_UUID_H_
+#ifndef _MCUUID_H_
+#define _MCUUID_H_
 
 #define UUID_TYPE
 
-/** Universally Unique Identifier (UUID) according to ISO/IEC 11578. */
+/* Universally Unique Identifier (UUID) according to ISO/IEC 11578. */
 struct mc_uuid_t {
-	uint8_t value[16]; /**< Value of the UUID. */
+	uint8_t		value[16];	/* Value of the UUID. */
 };
 
-/** UUID value used as free marker in service provider containers. */
+/* UUID value used as free marker in service provider containers. */
 #define MC_UUID_FREE_DEFINE \
 	{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, \
 	  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }
@@ -50,7 +46,7 @@
 	MC_UUID_FREE_DEFINE
 };
 
-/** Reserved UUID. */
+/* Reserved UUID. */
 #define MC_UUID_RESERVED_DEFINE \
 	{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \
 	  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
@@ -59,7 +55,7 @@
 	MC_UUID_RESERVED_DEFINE
 };
 
-/** UUID for system applications. */
+/* UUID for system applications. */
 #define MC_UUID_SYSTEM_DEFINE \
 	{ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, \
 	  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE }
@@ -68,7 +64,4 @@
 	MC_UUID_SYSTEM_DEFINE
 };
 
-#endif /* MC_UUID_H_ */
-
-/** @} */
-
+#endif /* _MCUUID_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/main.c b/drivers/gud/mobicore_kernelapi/main.c
index 62997f7..50359b1 100644
--- a/drivers/gud/mobicore_kernelapi/main.c
+++ b/drivers/gud/mobicore_kernelapi/main.c
@@ -1,7 +1,7 @@
-/**
+/*
  * MobiCore KernelApi module
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009-2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -14,6 +14,7 @@
 #include <linux/sched.h>
 #include <linux/netlink.h>
 #include <linux/kthread.h>
+#include <linux/device.h>
 #include <net/sock.h>
 
 #include <linux/list.h>
@@ -29,23 +30,27 @@
 	atomic_t counter;
 };
 
-struct mc_kernelapi_ctx *mod_ctx; /* = NULL; */
+struct mc_kernelapi_ctx *mod_ctx;
 
-/*----------------------------------------------------------------------------*/
+/* Define a MobiCore Kernel API device structure for use with dev_debug() etc */
+struct device_driver mc_kernel_api_name = {
+	.name = "mckernelapi"
+};
+
+struct device mc_kernel_api_subname = {
+	.init_name = "", /* Set to 'mcapi' at mcapi_init() time */
+	.driver = &mc_kernel_api_name
+};
+
+struct device *mc_kapi = &mc_kernel_api_subname;
+
 /* get a unique ID */
-unsigned int mcapi_unique_id(
-	void
-)
+unsigned int mcapi_unique_id(void)
 {
-	return (unsigned int)atomic_inc_return(
-		&(mod_ctx->counter));
+	return (unsigned int)atomic_inc_return(&(mod_ctx->counter));
 }
 
-
-/*----------------------------------------------------------------------------*/
-static struct connection *mcapi_find_connection(
-	uint32_t seq
-)
+static struct connection *mcapi_find_connection(uint32_t seq)
 {
 	struct connection *tmp;
 	struct list_head *pos;
@@ -60,24 +65,21 @@
 	return NULL;
 }
 
-/*----------------------------------------------------------------------------*/
-void mcapi_insert_connection(
-	struct connection *connection
-)
+void mcapi_insert_connection(struct connection *connection)
 {
 	list_add_tail(&(connection->list), &(mod_ctx->peers));
 	connection->socket_descriptor = mod_ctx->sk;
 }
 
-void mcapi_remove_connection(
-	uint32_t seq
-)
+void mcapi_remove_connection(uint32_t seq)
 {
 	struct connection *tmp;
 	struct list_head *pos, *q;
 
-	/* Delete all session objects. Usually this should not be needed as
-	   closeDevice() requires that all sessions have been closed before.*/
+	/*
+	 * Delete all session objects. Usually this should not be needed as
+	 * closeDevice() requires that all sessions have been closed before.
+	 */
 	list_for_each_safe(pos, q, &mod_ctx->peers) {
 		tmp = list_entry(pos, struct connection, list);
 		if (tmp->sequence_magic == seq) {
@@ -87,11 +89,7 @@
 	}
 }
 
-/*----------------------------------------------------------------------------*/
-static int mcapi_process(
-	struct sk_buff *skb,
-	struct nlmsghdr *nlh
-)
+static int mcapi_process(struct sk_buff *skb, struct nlmsghdr *nlh)
 {
 	struct connection *c;
 	int length;
@@ -102,13 +100,14 @@
 	pid = nlh->nlmsg_pid;
 	length = nlh->nlmsg_len;
 	seq = nlh->nlmsg_seq;
-	MCDRV_DBG_VERBOSE("nlmsg len %d type %d pid 0x%X seq %d\n",
-		   length, nlh->nlmsg_type, pid, seq);
+	MCDRV_DBG_VERBOSE(mc_kapi, "nlmsg len %d type %d pid 0x%X seq %d\n",
+			  length, nlh->nlmsg_type, pid, seq);
 	do {
 		c = mcapi_find_connection(seq);
 		if (!c) {
-			MCDRV_ERROR("Invalid incomming connection - seq=%u!",
-				seq);
+			MCDRV_ERROR(mc_kapi,
+				    "Invalid incoming connection - seq=%u!",
+				    seq);
 			ret = -1;
 			break;
 		}
@@ -121,10 +120,7 @@
 	return ret;
 }
 
-/*----------------------------------------------------------------------------*/
-static void mcapi_callback(
-	struct sk_buff *skb
-)
+static void mcapi_callback(struct sk_buff *skb)
 {
 	struct nlmsghdr *nlh = nlmsg_hdr(skb);
 	int len = skb->len;
@@ -141,19 +137,20 @@
 	}
 }
 
-/*----------------------------------------------------------------------------*/
 static int __init mcapi_init(void)
 {
-	printk(KERN_INFO "Mobicore API module initialized!\n");
+	dev_set_name(mc_kapi, "mcapi");
+
+	dev_info(mc_kapi, "Mobicore API module initialized!\n");
 
 	mod_ctx = kzalloc(sizeof(struct mc_kernelapi_ctx), GFP_KERNEL);
 
 	/* start kernel thread */
 	mod_ctx->sk = netlink_kernel_create(&init_net, MC_DAEMON_NETLINK, 0,
-					mcapi_callback, NULL, THIS_MODULE);
+					    mcapi_callback, NULL, THIS_MODULE);
 
 	if (!mod_ctx->sk) {
-		MCDRV_ERROR("register of recieve handler failed");
+		MCDRV_ERROR(mc_kapi, "register of receive handler failed");
 		return -EFAULT;
 	}
 
@@ -163,7 +160,7 @@
 
 static void __exit mcapi_exit(void)
 {
-	printk(KERN_INFO "Unloading Mobicore API module.\n");
+	dev_info(mc_kapi, "Unloading Mobicore API module.\n");
 
 	if (mod_ctx->sk != NULL) {
 		netlink_kernel_release(mod_ctx->sk);
diff --git a/drivers/gud/mobicore_kernelapi/public/mobicore_driver_api.h b/drivers/gud/mobicore_kernelapi/public/mobicore_driver_api.h
index ccfb2e5..07a3ae3 100644
--- a/drivers/gud/mobicore_kernelapi/public/mobicore_driver_api.h
+++ b/drivers/gud/mobicore_kernelapi/public/mobicore_driver_api.h
@@ -1,21 +1,10 @@
-/**
- * @defgroup MCD_API					MobiCore Driver API
- * @addtogroup MCD_API
- * @{
- *
- * @if DOXYGEN_MCDRV_API
- *   @mainpage MobiCore Driver API.
- * @endif
- *
+/*
  * MobiCore Driver API.
  *
  * The MobiCore (MC) Driver API provides access functions to the MobiCore
  * runtime environment and the contained Trustlets.
  *
- * @image html DoxyOverviewDrvApi500x.png
- * @image latex DoxyOverviewDrvApi500x.png "MobiCore Overview" width=12cm
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -41,278 +30,272 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef MCDRIVER_H_
-#define MCDRIVER_H_
+#ifndef _MOBICORE_DRIVER_API_H_
+#define _MOBICORE_DRIVER_API_H_
 
 #define __MC_CLIENT_LIB_API
 
 #include "mcuuid.h"
 
-/**
+/*
  * Return values of MobiCore driver functions.
  */
 enum mc_result {
-	/**< Function call succeeded. */
-	MC_DRV_OK		= 0,
-	/**< No notification available. */
-	MC_DRV_NO_NOTIFICATION	= 1,
-	/**< Error during notification on communication level. */
-	MC_DRV_ERR_NOTIFICATION	= 2,
-	/**< Function not implemented. */
-	MC_DRV_ERR_NOT_IMPLEMENTED = 3,
-	/**< No more resources available. */
-	MC_DRV_ERR_OUT_OF_RESOURCES = 4,
-	/**< Driver initialization failed. */
-	MC_DRV_ERR_INIT = 5,
-	/**< Unknown error. */
-	MC_DRV_ERR_UNKNOWN	= 6,
-	/**< The specified device is unknown. */
-	MC_DRV_ERR_UNKNOWN_DEVICE = 7,
-	/**< The specified session is unknown.*/
-	MC_DRV_ERR_UNKNOWN_SESSION = 8,
-	/**< The specified operation is not allowed. */
-	MC_DRV_ERR_INVALID_OPERATION = 9,
-	/**< The response header from the MC is invalid. */
-	MC_DRV_ERR_INVALID_RESPONSE = 10,
-	/**< Function call timed out. */
-	MC_DRV_ERR_TIMEOUT = 11,
-	/**< Can not allocate additional memory. */
-	MC_DRV_ERR_NO_FREE_MEMORY = 12,
-	/**< Free memory failed. */
-	MC_DRV_ERR_FREE_MEMORY_FAILED = 13,
-	/**< Still some open sessions pending. */
-	MC_DRV_ERR_SESSION_PENDING = 14,
-	/**< MC daemon not reachable */
-	MC_DRV_ERR_DAEMON_UNREACHABLE = 15,
-	/**< The device file of the kernel module could not be opened. */
-	MC_DRV_ERR_INVALID_DEVICE_FILE = 16,
-	/**< Invalid parameter. */
+	/* Function call succeeded. */
+	MC_DRV_OK			= 0,
+	/* No notification available. */
+	MC_DRV_NO_NOTIFICATION		= 1,
+	/* Error during notification on communication level. */
+	MC_DRV_ERR_NOTIFICATION		= 2,
+	/* Function not implemented. */
+	MC_DRV_ERR_NOT_IMPLEMENTED	= 3,
+	/* No more resources available. */
+	MC_DRV_ERR_OUT_OF_RESOURCES	= 4,
+	/* Driver initialization failed. */
+	MC_DRV_ERR_INIT			= 5,
+	/* Unknown error. */
+	MC_DRV_ERR_UNKNOWN		= 6,
+	/* The specified device is unknown. */
+	MC_DRV_ERR_UNKNOWN_DEVICE	= 7,
+	/* The specified session is unknown.*/
+	MC_DRV_ERR_UNKNOWN_SESSION	= 8,
+	/* The specified operation is not allowed. */
+	MC_DRV_ERR_INVALID_OPERATION	= 9,
+	/* The response header from the MC is invalid. */
+	MC_DRV_ERR_INVALID_RESPONSE	= 10,
+	/* Function call timed out. */
+	MC_DRV_ERR_TIMEOUT		= 11,
+	/* Can not allocate additional memory. */
+	MC_DRV_ERR_NO_FREE_MEMORY	= 12,
+	/* Free memory failed. */
+	MC_DRV_ERR_FREE_MEMORY_FAILED	= 13,
+	/* Still some open sessions pending. */
+	MC_DRV_ERR_SESSION_PENDING	= 14,
+	/* MC daemon not reachable */
+	MC_DRV_ERR_DAEMON_UNREACHABLE	= 15,
+	/* The device file of the kernel module could not be opened. */
+	MC_DRV_ERR_INVALID_DEVICE_FILE	= 16,
+	/* Invalid parameter. */
 	MC_DRV_ERR_INVALID_PARAMETER	= 17,
-	/**< Unspecified error from Kernel Module*/
-	MC_DRV_ERR_KERNEL_MODULE = 18,
-	/**< Error during mapping of additional bulk memory to session. */
-	MC_DRV_ERR_BULK_MAPPING = 19,
-	/**< Error during unmapping of additional bulk memory to session. */
-	MC_DRV_ERR_BULK_UNMAPPING = 20,
-	/**< Notification received, exit code available. */
-	MC_DRV_INFO_NOTIFICATION = 21,
-	/**< Set up of NWd connection failed. */
-	MC_DRV_ERR_NQ_FAILED = 22
+	/* Unspecified error from Kernel Module*/
+	MC_DRV_ERR_KERNEL_MODULE	= 18,
+	/* Error during mapping of additional bulk memory to session. */
+	MC_DRV_ERR_BULK_MAPPING		= 19,
+	/* Error during unmapping of additional bulk memory to session. */
+	MC_DRV_ERR_BULK_UNMAPPING	= 20,
+	/* Notification received, exit code available. */
+	MC_DRV_INFO_NOTIFICATION	= 21,
+	/* Set up of NWd connection failed. */
+	MC_DRV_ERR_NQ_FAILED		= 22
 };
 
-
-/**
+/*
  * Driver control command.
  */
 enum mc_driver_ctrl {
-	MC_CTRL_GET_VERSION = 1 /**< Return the driver version */
+	/* Return the driver version */
+	MC_CTRL_GET_VERSION		= 1
 };
 
-
-/** Structure of Session Handle, includes the Session ID and the Device ID the
+/*
+ * Structure of Session Handle, includes the Session ID and the Device ID the
  * Session belongs to.
  * The session handle will be used for session-based MobiCore communication.
  * It will be passed to calls which address a communication end point in the
  * MobiCore environment.
  */
 struct mc_session_handle {
-	uint32_t session_id; /**< MobiCore session ID */
-	uint32_t device_id; /**< Device ID the session belongs to */
+	uint32_t session_id;		/* MobiCore session ID */
+	uint32_t device_id;		/* Device ID the session belongs to */
 };
 
-/** Information structure about additional mapped Bulk buffer between the
- * Trustlet Connector (Nwd) and the Trustlet (Swd). This structure is
+/*
+ * Information structure about additional mapped Bulk buffer between the
+ * Trustlet Connector (NWd) and the Trustlet (SWd). This structure is
  * initialized from a Trustlet Connector by calling mc_map().
  * In order to use the memory within a Trustlet the Trustlet Connector has to
  * inform the Trustlet with the content of this structure via the TCI.
  */
 struct mc_bulk_map {
-	/**< The virtual address of the Bulk buffer regarding the address space
+	/* The virtual address of the Bulk buffer regarding the address space
 	 * of the Trustlet, already includes a possible offset! */
 	void *secure_virt_addr;
-	uint32_t secure_virt_len; /**< Length of the mapped Bulk buffer */
+	uint32_t secure_virt_len;	/* Length of the mapped Bulk buffer */
 };
 
-
-/**< The default device ID */
+/* The default device ID */
 #define MC_DEVICE_ID_DEFAULT	0
-/**< Wait infinite for a response of the MC. */
+/* Wait infinite for a response of the MC. */
 #define MC_INFINITE_TIMEOUT	((int32_t)(-1))
-/**< Do not wait for a response of the MC. */
+/* Do not wait for a response of the MC. */
 #define MC_NO_TIMEOUT		0
-/**< TCI/DCI must not exceed 1MiB */
+/* TCI/DCI must not exceed 1MiB */
 #define MC_MAX_TCI_LEN		0x100000
 
-
-
-/** Open a new connection to a MobiCore device.
+/**
+ * mc_open_device() - Open a new connection to a MobiCore device.
+ * @device_id:		Identifier for the MobiCore device to be used.
+ *			MC_DEVICE_ID_DEFAULT refers to the default device.
  *
- * mc_open_device() initializes all device specific resources required to
- * communicate with an MobiCore instance located on the specified device in the
- * system. If the device does not exist the function will return
- * MC_DRV_ERR_UNKNOWN_DEVICE.
+ * Initializes all device specific resources required to communicate with a
+ * MobiCore instance located on the specified device in the system. If the
+ * device does not exist the function will return MC_DRV_ERR_UNKNOWN_DEVICE.
  *
- * @param [in] device_id Identifier for the MobiCore device to be used.
- * MC_DEVICE_ID_DEFAULT refers to the default device.
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_ERR_INVALID_OPERATION if device already opened.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon occur.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device_id is unknown.
- * @return MC_DRV_ERR_INVALID_DEVICE_FILE if kernel module under
- * /dev/mobicore cannot be opened
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_ERR_INVALID_OPERATION:	device already opened
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device_id unknown
+ *	MC_DRV_ERR_INVALID_DEVICE_FILE:	kernel module under /dev/mobicore
+ *					cannot be opened
  */
-__MC_CLIENT_LIB_API enum mc_result mc_open_device(
-	uint32_t device_id
-);
+__MC_CLIENT_LIB_API enum mc_result mc_open_device(uint32_t device_id);
 
-/** Close the connection to a MobiCore device.
+/**
+ * mc_close_device() - Close the connection to a MobiCore device.
+ * @device_id:		Identifier for the MobiCore device.
+ *
  * When closing a device, active sessions have to be closed beforehand.
  * Resources associated with the device will be released.
  * The device may be opened again after it has been closed.
  *
- * @param [in] device_id Identifier for the MobiCore device.
  * MC_DEVICE_ID_DEFAULT refers to the default device.
  *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id is invalid.
- * @return MC_DRV_ERR_SESSION_PENDING when a session is still open.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon occur.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id is invalid
+ *	MC_DRV_ERR_SESSION_PENDING:	a session is still open
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon occur
  */
-__MC_CLIENT_LIB_API enum mc_result mc_close_device(
-	uint32_t device_id
-);
+__MC_CLIENT_LIB_API enum mc_result mc_close_device(uint32_t device_id);
 
-/** Open a new session to a Trustlet. The trustlet with the given UUID has
- * to be available in the flash filesystem.
+/**
+ * mc_open_session() - Open a new session to a Trustlet.
+ * @session:		On success, the session data will be returned
+ * @uuid:		UUID of the Trustlet to be opened
+ * @tci:		TCI buffer for communicating with the Trustlet
+ * @tci_len:		Length of the TCI buffer. Maximum allowed value
+ *			is MC_MAX_TCI_LEN
+ *
+ * The Trustlet with the given UUID has to be available in the flash filesystem.
  *
  * Write MCP open message to buffer and notify MobiCore about the availability
  * of a new command.
+ *
  * Waits till the MobiCore responses with the new session ID (stored in the MCP
  * buffer).
  *
- * @param [in,out] session On success, the session data will be returned.
  * Note that session.device_id has to be the device id of an opened device.
- * @param [in] uuid UUID of the Trustlet to be opened.
- * @param [in] tci TCI buffer for communicating with the trustlet.
- * @param [in] tci_len Length of the TCI buffer. Maximum allowed value
- * is MC_MAX_TCI_LEN.
  *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if session parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id is invalid.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon socket occur.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when daemon returns an error.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	session parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id is invalid
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon socket occur
+ *	MC_DRV_ERR_NQ_FAILED:		daemon returns an error
  */
 __MC_CLIENT_LIB_API enum mc_result mc_open_session(
-	struct mc_session_handle  *session,
-	const struct mc_uuid_t *uuid,
-	uint8_t *tci,
-	uint32_t tci_len
-);
+	struct mc_session_handle *session, const struct mc_uuid_t *uuid,
+	uint8_t *tci, uint32_t tci_len);
 
-/** Close a Trustlet session.
+/**
+ * mc_close_session() - Close a Trustlet session.
+ * @session:		Session to be closed.
  *
  * Closes the specified MobiCore session. The call will block until the
  * session has been closed.
  *
- * @pre Device device_id has to be opened in advance.
+ * Device device_id has to be opened in advance.
  *
- * @param [in] session Session to be closed.
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if session parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon occur.
- * @return MC_DRV_ERR_INVALID_DEVICE_FILE when daemon cannot open trustlet file.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	session parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon occur
+ *	MC_DRV_ERR_INVALID_DEVICE_FILE:	daemon cannot open Trustlet file
  */
 __MC_CLIENT_LIB_API enum mc_result mc_close_session(
-	struct mc_session_handle *session
-);
+	struct mc_session_handle *session);
 
-/** Notify a session.
+/**
+ * mc_notify() - Notify a session.
+ * @session:		The session to be notified.
+ *
  * Notifies the session end point about available message data.
  * If the session parameter is correct, notify will always succeed.
  * Corresponding errors can only be received by mc_wait_notification().
- * @pre A session has to be opened in advance.
  *
- * @param session The session to be notified.
+ * A session has to be opened in advance.
  *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if session parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	session parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
  */
-__MC_CLIENT_LIB_API enum mc_result mc_notify(
-	struct mc_session_handle *session
-);
+__MC_CLIENT_LIB_API enum mc_result mc_notify(struct mc_session_handle *session);
 
-/** Wait for a notification.
+/**
+ * mc_wait_notification() - Wait for a notification.
+ * @session:		The session the notification should correspond to.
+ * @timeout:		Time in milliseconds to wait
+ *			(MC_NO_TIMEOUT : direct return, > 0 : milliseconds,
+ *			 MC_INFINITE_TIMEOUT : wait infinitely)
  *
  * Wait for a notification issued by the MobiCore for a specific session.
  * The timeout parameter specifies the number of milliseconds the call will wait
  * for a notification.
+ *
  * If the caller passes 0 as timeout value the call will immediately return.
  * If timeout value is below 0 the call will block until a notification for the
- session has been received.
+ * session has been received.
  *
- * @attention if timeout is below 0, call will block:
+ * If timeout is below 0, call will block.
+ *
  * Caller has to trust the other side to send a notification to wake him up
  * again.
  *
- * @param [in] session The session the notification should correspond to.
- * @param [in] timeout Time in milliseconds to wait
- * (MC_NO_TIMEOUT : direct return, > 0 : milliseconds,
- * MC_INFINITE_TIMEOUT : wait infinitely)
- *
- * @return MC_DRV_OK if notification is available.
- * @return MC_DRV_ERR_TIMEOUT if no notification arrived in time.
- * @return MC_DRV_INFO_NOTIFICATION if a problem with the session was
- * encountered. Get more details with mc_get_session_error_code().
- * @return MC_DRV_ERR_NOTIFICATION if a problem with the socket occurred.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_ERR_TIMEOUT:		no notification arrived in time
+ *	MC_DRV_INFO_NOTIFICATION:	a problem with the session was
+ *					encountered. Get more details with
+ *					mc_get_session_error_code()
+ *	MC_DRV_ERR_NOTIFICATION:	a problem with the socket occurred
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
  */
 __MC_CLIENT_LIB_API enum mc_result mc_wait_notification(
-	struct mc_session_handle  *session,
-	int32_t			timeout
-);
+	struct mc_session_handle *session, int32_t timeout);
 
 /**
- * Allocate a block of world shared memory (WSM).
+ * mc_malloc_wsm() - Allocate a block of world shared memory (WSM).
+ * @device_id:		The ID of an opened device to retrieve the WSM from.
+ * @align:		The alignment (number of pages) of the memory block
+ *			(e.g. 0x00000001 for 4kb).
+ * @len:		Length of the block in bytes.
+ * @wsm:		Virtual address of the world shared memory block.
+ * @wsm_flags:		Platform specific flags describing the memory to
+ *			be allocated.
+ *
  * The MC driver allocates a contiguous block of memory which can be used as
  * WSM.
  * This implicates that the allocated memory is aligned according to the
  * alignment parameter.
+ *
  * Always returns a buffer of size WSM_SIZE aligned to 4K.
  *
- * @param [in]  device_id The ID of an opened device to retrieve the WSM from.
- * @param [in]  align The alignment (number of pages) of the memory block
- * (e.g. 0x00000001 for 4kb).
- * @param [in]  len	Length of the block in bytes.
- * @param [out] wsm Virtual address of the world shared memory block.
- * @param [in]  wsm_flags Platform specific flags describing the memory to
- * be allocated.
+ * Align and wsm_flags are currently ignored
  *
- * @attention: align and wsm_flags are currently ignored
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id is invalid.
- * @return MC_DRV_ERR_NO_FREE_MEMORY if no more contiguous memory is available
- * in this size or for this process.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id is invalid
+ *	MC_DRV_ERR_NO_FREE_MEMORY:	no more contiguous memory is
+ *					available in this size or for this
+ *					process
  */
 __MC_CLIENT_LIB_API enum mc_result mc_malloc_wsm(
 	uint32_t  device_id,
@@ -323,161 +306,140 @@
 );
 
 /**
- * Free a block of world shared memory (WSM).
+ * mc_free_wsm() - Free a block of world shared memory (WSM).
+ * @device_id:		The ID to which the given address belongs
+ * @wsm:		Address of WSM block to be freed
+ *
  * The MC driver will free a block of world shared memory (WSM) previously
  * allocated with mc_malloc_wsm(). The caller has to assure that the address
  * handed over to the driver is a valid WSM address.
  *
- * @param [in] device_id The ID to which the given address belongs.
- * @param [in] wsm Address of WSM block to be freed.
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id is invalid.
- * @return MC_DRV_ERR_FREE_MEMORY_FAILED on failures.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	when device id is invalid
+ *	MC_DRV_ERR_FREE_MEMORY_FAILED:	on failure
  */
-__MC_CLIENT_LIB_API enum mc_result mc_free_wsm(
-	uint32_t  device_id,
-	uint8_t   *wsm
-);
+__MC_CLIENT_LIB_API enum mc_result mc_free_wsm(uint32_t device_id,
+					       uint8_t *wsm);
 
 /**
- * Map additional bulk buffer between a Trustlet Connector (TLC) and
- * the Trustlet (TL) for a session.
+ *mc_map() -	Map additional bulk buffer between a Trustlet Connector (TLC)
+ *		and the Trustlet (TL) for a session
+ * @session:		Session handle with information of the device_id and
+ *			the session_id. The given buffer is mapped to the
+ *			session specified in the sessionHandle
+ * @buf:		Virtual address of a memory portion (relative to TLC)
+ *			to be shared with the Trustlet, already includes a
+ *			possible offset!
+ * @len:		length of buffer block in bytes.
+ * @map_info:		Information structure about the mapped Bulk buffer
+ *			between the TLC (NWd) and the TL (SWd).
+ *
  * Memory allocated in user space of the TLC can be mapped as additional
  * communication channel (besides TCI) to the Trustlet. Limitation of the
  * Trustlet memory structure apply: only 6 chunks can be mapped with a maximum
  * chunk size of 1 MiB each.
  *
- * @attention It is up to the application layer (TLC) to inform the Trustlet
+ * It is up to the application layer (TLC) to inform the Trustlet
  * about the additional mapped bulk memory.
  *
- * @param [in] session Session handle with information of the device_id and
- * the session_id. The
- * given buffer is mapped to the session specified in the sessionHandle.
- * @param [in] buf Virtual address of a memory portion (relative to TLC)
- * to be shared with the Trustlet, already includes a possible offset!
- * @param [in] len length of buffer block in bytes.
- * @param [out] map_info Information structure about the mapped Bulk buffer
- * between the TLC (Nwd) and
- * the TL (Swd).
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon occur.
- * @return MC_DRV_ERR_BULK_MAPPING when buf is already uses as bulk buffer or
- * when registering the buffer failed.
- *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon occur
+ *	MC_DRV_ERR_BULK_MAPPING:	buf is already uses as bulk buffer or
+ *					when registering the buffer failed
  */
 __MC_CLIENT_LIB_API enum mc_result mc_map(
-	struct mc_session_handle	*session,
-	void				*buf,
-	uint32_t			len,
-	struct mc_bulk_map		*map_info
-);
+	struct mc_session_handle *session, void	*buf, uint32_t len,
+	struct mc_bulk_map *map_info);
 
 /**
- * Remove additional mapped bulk buffer between Trustlet Connector (TLC)
- * and the Trustlet (TL) for a session.
+ * mc_unmap() -	Remove additional mapped bulk buffer between Trustlet Connector
+ *		(TLC) and the Trustlet (TL) for a session
+ * @session:		Session handle with information of the device_id and
+ *			the session_id. The given buffer is unmapped from the
+ *			session specified in the sessionHandle.
+ * @buf:		Virtual address of a memory portion (relative to TLC)
+ *			shared with the TL, already includes a possible offset!
+ * @map_info:		Information structure about the mapped Bulk buffer
+ *			between the TLC (NWd) and the TL (SWd)
  *
- * @attention The bulk buffer will immediately be unmapped from the session
- * context.
- * @attention The application layer (TLC) must inform the TL about unmapping
- * of the additional bulk memory before calling mc_unmap!
+ * The bulk buffer will immediately be unmapped from the session context.
  *
- * @param [in] session Session handle with information of the device_id and
- * the session_id. The given buffer is unmapped from the session specified
- * in the sessionHandle.
- * @param [in] buf Virtual address of a memory portion (relative to TLC)
- * shared with the TL, already includes a possible offset!
- * @param [in] map_info Information structure about the mapped Bulk buffer
- * between the TLC (Nwd) and
- * the TL (Swd).
- * @attention The clientlib currently ignores the len field in map_info.
+ * The application layer (TLC) must inform the TL about unmapping of the
+ * additional bulk memory before calling mc_unmap!
  *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
- * @return MC_DRV_ERR_DAEMON_UNREACHABLE when problems with daemon occur.
- * @return MC_DRV_ERR_BULK_UNMAPPING when buf was not registered earlier
- * or when unregistering failed.
+ * The clientlib currently ignores the len field in map_info.
  *
- * Uses a Mutex.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
+ *	MC_DRV_ERR_DAEMON_UNREACHABLE:	problems with daemon occur
+ *	MC_DRV_ERR_BULK_UNMAPPING:	buf was not registered earlier
+ *					or when unregistering failed
  */
 __MC_CLIENT_LIB_API enum mc_result mc_unmap(
-	struct mc_session_handle	*session,
-	void				*buf,
-	struct mc_bulk_map		*map_info
-);
-
+	struct mc_session_handle *session, void *buf,
+	struct mc_bulk_map *map_info);
 
 /**
- * @attention: Not implemented.
- * Execute driver specific command.
- * mc_driver_ctrl() can be used to execute driver specific commands.
- * Besides the control command MC_CTRL_GET_VERSION commands are implementation
- * specific.
+ * mc_driver_ctrl() - Execute driver specific command.
+ * @param:		Command ID of the command to be executed
+ * @data:		Command data and response depending on command
+ * @len:		Length of the data block
+ *
+ * Can be used to execute driver specific commands. Besides the control command
+ * MC_CTRL_GET_VERSION commands are implementation specific.
+ *
  * Please refer to the corresponding specification of the driver manufacturer.
  *
- * @param [in] param Command ID of the command to be executed.
- * @param [in, out] data  Command data and response depending on command.
- * @param [in] len Length of the data block.
- *
- * @return MC_DRV_ERR_NOT_IMPLEMENTED.
+ * Return codes:
+ *	MC_DRV_ERR_NOT_IMPLEMENTED.
  */
 __MC_CLIENT_LIB_API enum mc_result mc_driver_ctrl(
-	enum mc_driver_ctrl  param,
-	uint8_t		 *data,
-	uint32_t	len
-);
+	enum mc_driver_ctrl param, uint8_t *data, uint32_t len);
 
 /**
- * @attention: Not implemented.
- * Execute application management command.
- * mc_manage() shall be used to exchange application management commands with
- * the MobiCore.
+ * mc_manage() - Execute application management command.
+ * @device_id:		Identifier for the MobiCore device to be used.
+ *			NULL refers to the default device.
+ * @data:		Command data/response data depending on command
+ * @len:		Length of the data block
+ *
+ * Shall be used to exchange application management commands with the MobiCore.
  * The MobiCore Application Management Protocol is described in [MCAMP].
  *
- * @param [in] device_id Identifier for the MobiCore device to be used.
- * NULL refers to the default device.
- * @param [in, out] data  Command data/response data depending on command.
- * @param [in] len Length of the data block.
- *
- * @return MC_DRV_ERR_NOT_IMPLEMENTED.
+ * Return codes:
+ *	MC_DRV_ERR_NOT_IMPLEMENTED.
  */
 __MC_CLIENT_LIB_API enum mc_result mc_manage(
-	uint32_t  device_id,
-	uint8_t   *data,
-	uint32_t  len
-);
+	uint32_t device_id, uint8_t *data, uint32_t len);
 
 /**
- * Get additional error information of the last error that occured on a session.
+ * mc_get_session_error_code() - Get additional error information of the last
+ *				 error that occurred on a session.
+ * @session:		Session handle with information of the device_id and
+ *			the session_id
+ * @last_error:		>0 Trustlet has terminated itself with this value,
+ *			<0 Trustlet is dead because of an error within the
+ *			MobiCore (e.g. Kernel exception). See also MCI
+ *			definition.
+ *
  * After the request the stored error code will be deleted.
  *
- * @param [in] session Session handle with information of the device_id and
- * the session_id.
- * @param [out] last_error >0 Trustlet has terminated itself with this value,
- * <0 Trustlet is dead because of an error within the MobiCore
- * (e.g. Kernel exception).
- * See also MCI definition.
- *
- * @return MC_DRV_OK if operation has been successfully completed.
- * @return MC_DRV_INVALID_PARAMETER if a parameter is invalid.
- * @return MC_DRV_ERR_UNKNOWN_SESSION when session id is invalid.
- * @return MC_DRV_ERR_UNKNOWN_DEVICE when device id of session is invalid.
+ * Return codes:
+ *	MC_DRV_OK:			operation completed successfully
+ *	MC_DRV_INVALID_PARAMETER:	a parameter is invalid
+ *	MC_DRV_ERR_UNKNOWN_SESSION:	session id is invalid
+ *	MC_DRV_ERR_UNKNOWN_DEVICE:	device id of session is invalid
  */
 __MC_CLIENT_LIB_API enum mc_result mc_get_session_error_code(
-	struct mc_session_handle  *session,
-	int32_t			*last_error
-);
+	struct mc_session_handle *session, int32_t *last_error);
 
-#endif /** MCDRIVER_H_ */
-
-/** @} */
+#endif /* _MOBICORE_DRIVER_API_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/public/mobicore_driver_cmd.h b/drivers/gud/mobicore_kernelapi/public/mobicore_driver_cmd.h
index 9ff7989..3b8eb4b 100644
--- a/drivers/gud/mobicore_kernelapi/public/mobicore_driver_cmd.h
+++ b/drivers/gud/mobicore_kernelapi/public/mobicore_driver_cmd.h
@@ -1,8 +1,5 @@
-/** @addtogroup MCD_MCDIMPL_DAEMON
- * @{
- * @file
- *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -28,11 +25,8 @@
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef MCDAEMON_H_
-#define MCDAEMON_H_
-
-
-
+#ifndef _MOBICORE_DRIVER_CMD_H_
+#define _MOBICORE_DRIVER_CMD_H_
 
 #include "mcuuid.h"
 
@@ -64,19 +58,17 @@
 
 
 struct mc_drv_command_header_t {
-	uint32_t  command_id;
+	uint32_t command_id;
 };
 
 struct mc_drv_response_header_t {
-	uint32_t  response_id;
+	uint32_t response_id;
 };
 
-#define MC_DEVICE_ID_DEFAULT	0 /**< The default device ID */
+#define MC_DEVICE_ID_DEFAULT	0		/* The default device ID */
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_open_device_payload_t {
-	uint32_t  device_id;
+	uint32_t device_id;
 };
 
 struct mc_drv_cmd_open_device_t {
@@ -94,13 +86,13 @@
 	struct mc_drv_rsp_open_device_payload_t payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_close_device_t {
 	struct mc_drv_command_header_t header;
-	/* no payload here because close has none.
-	   If we use an empty struct, C++ will count it as 4 bytes.
-	   This will write too much into the socket at write(cmd,sizeof(cmd)) */
+	/*
+	 * no payload here because close has none.
+	 * If we use an empty struct, C++ will count it as 4 bytes.
+	 * This will write too much into the socket at write(cmd,sizeof(cmd))
+	 */
 };
 
 
@@ -113,8 +105,6 @@
 	struct mc_drv_rsp_close_device_payload_t payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_open_session_payload_t {
 	uint32_t device_id;
 	struct mc_uuid_t uuid;
@@ -141,8 +131,6 @@
 	struct mc_drv_rsp_open_session_payload_t  payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_close_session_payload_t {
 	uint32_t  session_id;
 };
@@ -162,8 +150,6 @@
 	struct mc_drv_rsp_close_session_payload_t payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_notify_payload_t {
 	uint32_t session_id;
 };
@@ -183,10 +169,9 @@
 	struct mc_drv_rsp_notify_payload_t  payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_map_bulk_mem_payload_t {
 	uint32_t session_id;
+	uint32_t handle;
 	uint32_t phys_addr_l2;
 	uint32_t offset_payload;
 	uint32_t len_bulk_mem;
@@ -209,10 +194,9 @@
 	struct mc_drv_rsp_map_bulk_mem_payload_t  payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_unmap_bulk_mem_payload_t {
 	uint32_t session_id;
+	uint32_t handle;
 	uint32_t secure_virtual_adr;
 	uint32_t len_bulk_mem;
 };
@@ -234,8 +218,6 @@
 	struct mc_drv_rsp_unmap_bulk_mem_payload_t payload;
 };
 
-
-/*****************************************************************************/
 struct mc_drv_cmd_nqconnect_payload_t {
 	uint32_t device_id;
 	uint32_t session_id;
@@ -258,8 +240,6 @@
 	struct mc_drv_rsp_nqconnect_payload_t payload;
 };
 
-
-/*****************************************************************************/
 union mc_drv_command_t {
 	struct mc_drv_command_header_t		header;
 	struct mc_drv_cmd_open_device_t		mc_drv_cmd_open_device;
@@ -284,6 +264,4 @@
 	struct mc_drv_rsp_unmap_bulk_mem_t	mc_drv_rsp_unmap_bulk_mem;
 };
 
-#endif /* MCDAEMON_H_ */
-
-/** @} */
+#endif /* _MOBICORE_DRIVER_CMD_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/session.c b/drivers/gud/mobicore_kernelapi/session.c
index e62b4b3..dae2c00 100644
--- a/drivers/gud/mobicore_kernelapi/session.c
+++ b/drivers/gud/mobicore_kernelapi/session.c
@@ -1,7 +1,5 @@
-/** @addtogroup MCD_IMPL_LIB
- * @{
- * @file
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -9,39 +7,35 @@
  */
 #include <linux/types.h>
 #include <linux/slab.h>
+#include <linux/device.h>
 #include "mc_kernel_api.h"
 #include "public/mobicore_driver_api.h"
 
 #include "session.h"
 
-/*****************************************************************************/
 struct bulk_buffer_descriptor *bulk_buffer_descriptor_create(
-	void		*virt_addr,
-	uint32_t	len,
-	uint32_t	handle,
-	void		*phys_addr_wsm_l2
-) {
-	struct bulk_buffer_descriptor *desc =
-		kzalloc(sizeof(struct bulk_buffer_descriptor), GFP_KERNEL);
+	void *virt_addr, uint32_t len, uint32_t handle, void *phys_addr_wsm_l2)
+{
+	struct bulk_buffer_descriptor *desc;
+
+	desc = kzalloc(sizeof(*desc), GFP_KERNEL);
 	desc->virt_addr = virt_addr;
 	desc->len = len;
 	desc->handle = handle;
 	desc->phys_addr_wsm_l2 = phys_addr_wsm_l2;
+
 	return desc;
 }
 
-/*****************************************************************************/
 struct session *session_create(
-	uint32_t	session_id,
-	void		*instance,
-	struct connection *connection
-) {
-	struct session *session =
-			kzalloc(sizeof(struct session), GFP_KERNEL);
+	uint32_t session_id, void *instance, struct connection *connection)
+{
+	struct session *session;
+
+	session = kzalloc(sizeof(*session), GFP_KERNEL);
 	session->session_id = session_id;
 	session->instance = instance;
 	session->notification_connection = connection;
-
 	session->session_info.last_error = SESSION_ERR_NO;
 	session->session_info.state = SESSION_STATE_INITIAL;
 
@@ -49,29 +43,31 @@
 	return session;
 }
 
-
-/*****************************************************************************/
-void session_cleanup(
-	struct session *session
-) {
-	struct bulk_buffer_descriptor  *bulk_buf_descr;
+void session_cleanup(struct session *session)
+{
+	struct bulk_buffer_descriptor *bulk_buf_descr;
 	struct list_head *pos, *q;
+	unsigned int phys_addr_wsm_l2;
 
 	/* Unmap still mapped buffers */
 	list_for_each_safe(pos, q, &session->bulk_buffer_descriptors) {
 		bulk_buf_descr =
 			list_entry(pos, struct bulk_buffer_descriptor, list);
 
-		MCDRV_DBG_VERBOSE("Physical Address of L2 Table = 0x%X, "
-				"handle= %d",
-				(unsigned int)bulk_buf_descr->phys_addr_wsm_l2,
-				bulk_buf_descr->handle);
+		phys_addr_wsm_l2 =
+			(unsigned int)bulk_buf_descr->phys_addr_wsm_l2;
+
+		MCDRV_DBG_VERBOSE(mc_kapi,
+				  "Phys Addr of L2 Table = 0x%X, handle= %d",
+				  phys_addr_wsm_l2,
+				  bulk_buf_descr->handle);
 
 		/* ignore any error, as we cannot do anything in this case. */
 		int ret = mobicore_unmap_vmem(session->instance,
-						bulk_buf_descr->handle);
+					      bulk_buf_descr->handle);
 		if (ret != 0)
-			MCDRV_DBG_ERROR("mobicore_unmap_vmem failed: %d", ret);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"mobicore_unmap_vmem failed: %d", ret);
 
 		list_del(pos);
 		kfree(bulk_buf_descr);
@@ -82,36 +78,27 @@
 	kfree(session);
 }
 
-
-/*****************************************************************************/
-void session_set_error_info(
-	struct session *session,
-	int32_t   err
-) {
+void session_set_error_info(struct session *session, int32_t err)
+{
 	session->session_info.last_error = err;
 }
 
-
-/*****************************************************************************/
-int32_t session_get_last_err(
-	struct session *session
-) {
+int32_t session_get_last_err(struct session *session)
+{
 	return session->session_info.last_error;
 }
 
-
-/*****************************************************************************/
-struct bulk_buffer_descriptor *session_add_bulk_buf(
-	struct session	*session,
-	void		*buf,
-	uint32_t	len
-) {
+struct bulk_buffer_descriptor *session_add_bulk_buf(struct session *session,
+						    void *buf, uint32_t len)
+{
 	struct bulk_buffer_descriptor *bulk_buf_descr = NULL;
-	struct bulk_buffer_descriptor  *tmp;
+	struct bulk_buffer_descriptor *tmp;
 	struct list_head *pos;
 
-	/* Search bulk buffer descriptors for existing vAddr
-	   At the moment a virtual address can only be added one time */
+	/*
+	 * Search bulk buffer descriptors for existing vAddr
+	 * At the moment a virtual address can only be added one time
+	 */
 	list_for_each(pos, &session->bulk_buffer_descriptors) {
 		tmp = list_entry(pos, struct bulk_buffer_descriptor, list);
 		if (tmp->virt_addr == buf)
@@ -119,84 +106,96 @@
 	}
 
 	do {
-		/* Prepare the interface structure for memory registration in
-		   Kernel Module */
-		void	*l2_table_phys;
-		uint32_t  handle;
+		/*
+		 * Prepare the interface structure for memory registration in
+		 * Kernel Module
+		 */
+		uint32_t l2_table_phys;
+		uint32_t handle;
 
-		int ret = mobicore_map_vmem(session->instance,
-					buf,
-					len,
-					&handle,
-					&l2_table_phys);
+		int ret = mobicore_map_vmem(session->instance, buf, len,
+					    &handle, &l2_table_phys);
 
 		if (ret != 0) {
-			MCDRV_DBG_ERROR("mobicore_map_vmem failed, ret=%d",
+			MCDRV_DBG_ERROR(mc_kapi,
+					"mobicore_map_vmem failed, ret=%d",
 					ret);
 			break;
 		}
 
-		MCDRV_DBG_VERBOSE("Physical Address of L2 Table = 0x%X, "
-				"handle=%d",
-				(unsigned int)l2_table_phys,
-				handle);
+		MCDRV_DBG_VERBOSE(mc_kapi,
+				  "Phys Addr of L2 Table = 0x%X, handle=%d",
+				  (unsigned int)l2_table_phys, handle);
 
 		/* Create new descriptor */
-		bulk_buf_descr = bulk_buffer_descriptor_create(
-							buf,
-							len,
-							handle,
-							l2_table_phys);
+		bulk_buf_descr =
+			bulk_buffer_descriptor_create(buf, len,
+						      handle,
+						      (void *)l2_table_phys);
 
 		/* Add to vector of descriptors */
 		list_add_tail(&(bulk_buf_descr->list),
-			&(session->bulk_buffer_descriptors));
+			      &(session->bulk_buffer_descriptors));
 	} while (0);
 
 	return bulk_buf_descr;
 }
 
-
-/*****************************************************************************/
-bool session_remove_bulk_buf(
-	struct session *session,
-	void	*virt_addr
-) {
+bool session_remove_bulk_buf(struct session *session, void *virt_addr)
+{
 	bool ret = true;
-	struct bulk_buffer_descriptor  *bulk_buf_descr = NULL;
-	struct bulk_buffer_descriptor  *tmp;
+	struct bulk_buffer_descriptor *bulk_buf = NULL;
+	struct bulk_buffer_descriptor *tmp;
 	struct list_head *pos, *q;
 
-	MCDRV_DBG_VERBOSE("Virtual Address = 0x%X", (unsigned int) virt_addr);
+	MCDRV_DBG_VERBOSE(mc_kapi, "Virtual Address = 0x%X",
+			  (unsigned int) virt_addr);
 
 	/* Search and remove bulk buffer descriptor */
 	list_for_each_safe(pos, q, &session->bulk_buffer_descriptors) {
 		tmp = list_entry(pos, struct bulk_buffer_descriptor, list);
 		if (tmp->virt_addr == virt_addr) {
-			bulk_buf_descr = tmp;
+			bulk_buf = tmp;
 			list_del(pos);
 			break;
 		}
 	}
 
-	if (bulk_buf_descr == NULL) {
-		MCDRV_DBG_ERROR("Virtual Address not found");
+	if (bulk_buf == NULL) {
+		MCDRV_DBG_ERROR(mc_kapi, "Virtual Address not found");
 		ret = false;
 	} else {
-		MCDRV_DBG_VERBOSE("WsmL2 phys=0x%X, handle=%d",
-			(unsigned int)bulk_buf_descr->phys_addr_wsm_l2,
-			bulk_buf_descr->handle);
+		MCDRV_DBG_VERBOSE(mc_kapi, "WsmL2 phys=0x%X, handle=%d",
+				  (unsigned int)bulk_buf->phys_addr_wsm_l2,
+				  bulk_buf->handle);
 
 		/* ignore any error, as we cannot do anything */
 		int ret = mobicore_unmap_vmem(session->instance,
-						bulk_buf_descr->handle);
+					      bulk_buf->handle);
 		if (ret != 0)
-			MCDRV_DBG_ERROR("mobicore_unmap_vmem failed: %d", ret);
+			MCDRV_DBG_ERROR(mc_kapi,
+					"mobicore_unmap_vmem failed: %d", ret);
 
-		kfree(bulk_buf_descr);
+		kfree(bulk_buf);
 	}
 
 	return ret;
 }
 
-/** @} */
+uint32_t session_find_bulk_buf(struct session *session, void *virt_addr)
+{
+	struct bulk_buffer_descriptor *tmp;
+	struct list_head *pos, *q;
+
+	MCDRV_DBG_VERBOSE(mc_kapi, "Virtual Address = 0x%X",
+			  (unsigned int) virt_addr);
+
+	/* Search and return buffer descriptor handle */
+	list_for_each_safe(pos, q, &session->bulk_buffer_descriptors) {
+		tmp = list_entry(pos, struct bulk_buffer_descriptor, list);
+		if (tmp->virt_addr == virt_addr)
+			return tmp->handle;
+	}
+
+	return 0;
+}
diff --git a/drivers/gud/mobicore_kernelapi/session.h b/drivers/gud/mobicore_kernelapi/session.h
index 9a53740..4a834e5 100644
--- a/drivers/gud/mobicore_kernelapi/session.h
+++ b/drivers/gud/mobicore_kernelapi/session.h
@@ -1,14 +1,12 @@
-/** @addtogroup MCD_IMPL_LIB
- * @{
- * @file
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+/*
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef SESSION_H_
-#define SESSION_H_
+#ifndef _MC_KAPI_SESSION_H_
+#define _MC_KAPI_SESSION_H_
 
 #include "common.h"
 
@@ -17,23 +15,27 @@
 
 
 struct bulk_buffer_descriptor {
-	void		*virt_addr;/**< The virtual address of the Bulk buffer*/
-	uint32_t	len;	  /**< Length of the Bulk buffer*/
+	void		*virt_addr;	/* The VA of the Bulk buffer */
+	uint32_t	len;		/* Length of the Bulk buffer */
 	uint32_t	handle;
-	void		*phys_addr_wsm_l2; /**< The physical address of the
-				L2 table of the Bulk buffer*/
-	struct list_head list; /**< The list param for using the kernel lists*/
+
+	/* The physical address of the L2 table of the Bulk buffer*/
+	void		*phys_addr_wsm_l2;
+
+	/* The list param for using the kernel lists*/
+	struct list_head list;
 };
 
 struct bulk_buffer_descriptor *bulk_buffer_descriptor_create(
-	void	*virt_addr,
-	uint32_t  len,
-	uint32_t  handle,
-	void	*phys_addr_wsm_l2
+	void		*virt_addr,
+	uint32_t	len,
+	uint32_t	handle,
+	void		*phys_addr_wsm_l2
 );
 
-/** Session states.
- * At the moment not used !!.
+/*
+ * Session states.
+ * At the moment not used !!
  */
 enum session_state {
 	SESSION_STATE_INITIAL,
@@ -41,96 +43,102 @@
 	SESSION_STATE_TRUSTLET_DEAD
 };
 
-#define SESSION_ERR_NO	  0 /**< No session error */
+#define SESSION_ERR_NO	0		/* No session error */
 
-/** Session information structure.
+/*
+ * Session information structure.
  * The information structure is used to hold the state of the session, which
  * will limit further actions for the session.
  * Also the last error code will be stored till it's read.
  */
 struct session_information {
-	enum session_state state;	   /**< Session state */
-	int32_t		last_error;	 /**< Last error of session */
+	enum session_state state;	/* Session state */
+	int32_t		last_error;	/* Last error of session */
 };
 
 
 struct session {
 	struct mc_instance		*instance;
-	/**< Descriptors of additional bulk buffer of a session */
-	struct list_head	bulk_buffer_descriptors;
-	/**< Informations about session */
-	struct session_information	 session_info;
 
-	uint32_t		 session_id;
-	struct connection	 *notification_connection;
+	/* Descriptors of additional bulk buffer of a session */
+	struct list_head		bulk_buffer_descriptors;
 
-	/**< The list param for using the kernel lists*/
-	struct list_head list;
+	/* Information about session */
+	struct session_information	session_info;
+
+	uint32_t			session_id;
+	struct connection		*notification_connection;
+
+	/* The list param for using the kernel lists */
+	struct list_head		list;
 };
 
 struct session *session_create(
-	uint32_t	 session_id,
-	void		 *instance,
-	struct connection *connection
+	uint32_t		session_id,
+	void			*instance,
+	struct connection	*connection
 );
 
-void session_cleanup(
-	struct session *session
-);
+void session_cleanup(struct session *session);
 
-/**
-  * Add address information of additional bulk buffer memory to session and
-  * register virtual memory in kernel module.
-  *
-  * @attention The virtual address can only be added one time. If the virtual
-  * address already exist, NULL is returned.
-  *
-  * @param buf The virtual address of bulk buffer.
-  * @param len Length of bulk buffer.
-  *
-  * @return On success the actual Bulk buffer descriptor with all address
-  * information is retured, NULL if an error occurs.
-  */
+/*
+ * session_add_bulk_buf() -	Add address information of additional bulk
+ *				buffer memory to session and register virtual
+ *				memory in kernel module
+ * @session:		Session information structure
+ * @buf:		The virtual address of bulk buffer.
+ * @len:		Length of bulk buffer.
+ *
+ * The virtual address can only be added one time. If the virtual
+ * address already exist, NULL is returned.
+ *
+ * On success the actual Bulk buffer descriptor with all address information
+ * is returned, NULL if an error occurs.
+ */
 struct bulk_buffer_descriptor *session_add_bulk_buf(
-	struct session *session,
-	void	*buf,
-	uint32_t  len
-);
+	struct session *session, void *buf, uint32_t len);
 
-/**
-  * Remove address information of additional bulk buffer memory from session and
-  * unregister virtual memory in kernel module
-  *
-  * @param buf The virtual address of the bulk buffer.
-  *
-  * @return true on success.
-  */
-bool session_remove_bulk_buf(
-	struct session *session,
-	void  *buf
-);
+/*
+ * session_remove_bulk_buf() -	Remove address information of additional bulk
+ *				buffer memory from session and unregister
+ *				virtual memory in kernel module
+ * @session:		Session information structure
+ * @buf:		The virtual address of the bulk buffer
+ *
+ * Returns true on success
+ */
+bool session_remove_bulk_buf(struct session *session, void *buf);
 
-/**
-  * Set additional error information of the last error that occured.
-  *
-  * @param errorCode The actual error.
-  */
-void session_set_error_info(
-	struct session *session,
-	int32_t err
-);
 
-/**
-  * Get additional error information of the last error that occured.
-  *
-  * @attention After request the information is set to SESSION_ERR_NO.
-  *
-  * @return Last stored error code or SESSION_ERR_NO.
-  */
-int32_t session_get_last_err(
-	struct session *session
-);
+/*
+ * session_find_bulk_buf() - Find the handle of the bulk buffer for this
+ *			session
+ *
+ * @session:		Session information structure
+ * @buf:		The virtual address of bulk buffer.
+ *
+ * On success the actual Bulk buffer handle is returned, 0
+ * if an error occurs.
+ */
+uint32_t session_find_bulk_buf(struct session *session, void *virt_addr);
 
-#endif /* SESSION_H_ */
+/*
+ * session_set_error_info() -	Set additional error information of the last
+ *				error that occurred.
+ * @session:		Session information structure
+ * @err:		The actual error
+ */
+void session_set_error_info(struct session *session, int32_t err);
 
-/** @} */
+/*
+ * session_get_last_err() -	Get additional error information of the last
+ *				error that occurred.
+ * @session:		Session information structure
+ *
+ * After request the information is set to SESSION_ERR_NO.
+ *
+ * Returns the last stored error code or SESSION_ERR_NO
+ */
+int32_t session_get_last_err(struct session *session);
+
+#endif /* _MC_KAPI_SESSION_H_ */
diff --git a/drivers/gud/mobicore_kernelapi/wsm.h b/drivers/gud/mobicore_kernelapi/wsm.h
index 6877c53..f8a107c 100644
--- a/drivers/gud/mobicore_kernelapi/wsm.h
+++ b/drivers/gud/mobicore_kernelapi/wsm.h
@@ -1,35 +1,33 @@
-/** @addtogroup MCD_MCDIMPL_DAEMON_SRV
- * @{
- * @file
- *
+/*
  * World shared memory definitions.
  *
- * <!-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
+ * <-- Copyright Giesecke & Devrient GmbH 2009 - 2012 -->
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
-#ifndef WSM_H_
-#define WSM_H_
+#ifndef _MC_KAPI_WSM_H_
+#define _MC_KAPI_WSM_H_
 
 #include "common.h"
 #include <linux/list.h>
 
 struct wsm {
-	void *virt_addr;
-	uint32_t len;
-	uint32_t handle;
-	void *phys_addr;
-	struct list_head list;
+	void			*virt_addr;
+	uint32_t		len;
+	uint32_t		handle;
+	void			*phys_addr;
+	struct list_head	list;
 };
 
 struct wsm *wsm_create(
-	void	*virt_addr,
-	uint32_t  len,
-	uint32_t  handle,
-	void	*phys_addr /*= NULL this may be unknown, so is can be omitted.*/
-);
-#endif /* WSM_H_ */
+	void			*virt_addr,
+	uint32_t		len,
+	uint32_t		handle,
 
-/** @} */
+	/* NULL this may be unknown, so is can be omitted */
+	void			*phys_addr
+);
+
+#endif /* _MC_KAPI_WSM_H_ */
diff --git a/drivers/hwmon/epm_adc.c b/drivers/hwmon/epm_adc.c
index 69a2f1c..a526f24 100644
--- a/drivers/hwmon/epm_adc.c
+++ b/drivers/hwmon/epm_adc.c
@@ -95,7 +95,8 @@
 
 #define EPM_PSOC_GLOBAL_ENABLE				81
 #define EPM_PSOC_VREF_VOLTAGE				2048
-#define EPM_PSOC_MAX_ADC_CODE_16_BIT			32767
+#define EPM_PSOC_MAX_ADC_CODE_15_BIT			32767
+#define EPM_PSOC_MAX_ADC_CODE_12_BIT			4096
 #define EPM_GLOBAL_ENABLE_MIN_DELAY			5000
 #define EPM_GLOBAL_ENABLE_MAX_DELAY			5100
 
@@ -590,13 +591,23 @@
 	struct epm_adc_drv *epm_adc = epm_adc_drv;
 	int32_t result_cur;
 
-	/* result = 2.048V/(32767 * gain * rsense) */
-	result_cur = (((EPM_PSOC_VREF_VOLTAGE * result)/
-				EPM_PSOC_MAX_ADC_CODE_16_BIT) * 1000);
+	if ((1 << index) & epm_adc->channel_mask) {
+		/* result = (2.048V * code)/(4096 * gain * rsense) */
+		result_cur = ((EPM_PSOC_VREF_VOLTAGE * result)/
+				EPM_PSOC_MAX_ADC_CODE_12_BIT);
 
-	result_cur = (result_cur/
+		result_cur = (result_cur/
+			(epm_adc->epm_psoc_ch_prop[index].gain *
+			epm_adc->epm_psoc_ch_prop[index].resistorvalue));
+	} else {
+		/* result = (2.048V * code)/(32767 * gain * rsense) */
+		result_cur = (((EPM_PSOC_VREF_VOLTAGE * result)/
+				EPM_PSOC_MAX_ADC_CODE_15_BIT) * 1000);
+
+		result_cur = (result_cur/
 		(epm_adc->epm_psoc_ch_prop[index].gain *
 			epm_adc->epm_psoc_ch_prop[index].resistorvalue));
+	}
 
 	return result_cur;
 }
@@ -1324,6 +1335,10 @@
 				return -EINVAL;
 			}
 
+			psoc_get_data.reading_value = epm_psoc_scale_result(
+				psoc_get_data.reading_value,
+				psoc_get_data.chan_num);
+
 			if (copy_to_user((void __user *)arg, &psoc_get_data,
 				sizeof(struct epm_psoc_get_data)))
 				return -EFAULT;
@@ -1529,38 +1544,38 @@
 }
 
 static struct sensor_device_attribute epm_adc_psoc_in_attrs[] = {
-	SENSOR_ATTR(ads0_chan0,  S_IRUGO, epm_adc_psoc_show_in, NULL, 0),
-	SENSOR_ATTR(ads0_chan1,  S_IRUGO, epm_adc_psoc_show_in, NULL, 1),
-	SENSOR_ATTR(ads0_chan2,  S_IRUGO, epm_adc_psoc_show_in, NULL, 2),
-	SENSOR_ATTR(ads0_chan3,  S_IRUGO, epm_adc_psoc_show_in, NULL, 3),
-	SENSOR_ATTR(ads0_chan4,  S_IRUGO, epm_adc_psoc_show_in, NULL, 4),
-	SENSOR_ATTR(ads0_chan5,  S_IRUGO, epm_adc_psoc_show_in, NULL, 5),
-	SENSOR_ATTR(ads0_chan6,  S_IRUGO, epm_adc_psoc_show_in, NULL, 6),
-	SENSOR_ATTR(ads0_chan7,  S_IRUGO, epm_adc_psoc_show_in, NULL, 7),
-	SENSOR_ATTR(ads0_chan8,  S_IRUGO, epm_adc_psoc_show_in, NULL, 8),
-	SENSOR_ATTR(ads0_chan9,  S_IRUGO, epm_adc_psoc_show_in, NULL, 9),
-	SENSOR_ATTR(ads0_chan10, S_IRUGO, epm_adc_psoc_show_in, NULL, 10),
-	SENSOR_ATTR(ads0_chan11, S_IRUGO, epm_adc_psoc_show_in, NULL, 11),
-	SENSOR_ATTR(ads0_chan12, S_IRUGO, epm_adc_psoc_show_in, NULL, 12),
-	SENSOR_ATTR(ads0_chan13, S_IRUGO, epm_adc_psoc_show_in, NULL, 13),
-	SENSOR_ATTR(ads0_chan14, S_IRUGO, epm_adc_psoc_show_in, NULL, 14),
-	SENSOR_ATTR(ads0_chan15, S_IRUGO, epm_adc_psoc_show_in, NULL, 15),
-	SENSOR_ATTR(ads1_chan0,  S_IRUGO, epm_adc_psoc_show_in, NULL, 16),
-	SENSOR_ATTR(ads1_chan1,  S_IRUGO, epm_adc_psoc_show_in, NULL, 17),
-	SENSOR_ATTR(ads1_chan2,  S_IRUGO, epm_adc_psoc_show_in, NULL, 18),
-	SENSOR_ATTR(ads1_chan3,  S_IRUGO, epm_adc_psoc_show_in, NULL, 19),
-	SENSOR_ATTR(ads1_chan4,  S_IRUGO, epm_adc_psoc_show_in, NULL, 20),
-	SENSOR_ATTR(ads1_chan5,  S_IRUGO, epm_adc_psoc_show_in, NULL, 21),
-	SENSOR_ATTR(ads1_chan6,  S_IRUGO, epm_adc_psoc_show_in, NULL, 22),
-	SENSOR_ATTR(ads1_chan7,  S_IRUGO, epm_adc_psoc_show_in, NULL, 23),
-	SENSOR_ATTR(ads1_chan8,  S_IRUGO, epm_adc_psoc_show_in, NULL, 24),
-	SENSOR_ATTR(ads1_chan9,  S_IRUGO, epm_adc_psoc_show_in, NULL, 25),
-	SENSOR_ATTR(ads1_chan10, S_IRUGO, epm_adc_psoc_show_in, NULL, 26),
-	SENSOR_ATTR(ads1_chan11, S_IRUGO, epm_adc_psoc_show_in, NULL, 27),
-	SENSOR_ATTR(ads1_chan12, S_IRUGO, epm_adc_psoc_show_in, NULL, 28),
-	SENSOR_ATTR(ads1_chan13, S_IRUGO, epm_adc_psoc_show_in, NULL, 29),
-	SENSOR_ATTR(ads1_chan14, S_IRUGO, epm_adc_psoc_show_in, NULL, 30),
-	SENSOR_ATTR(ads1_chan15, S_IRUGO, epm_adc_psoc_show_in, NULL, 31),
+	SENSOR_ATTR(psoc0_chan0,  S_IRUGO, epm_adc_psoc_show_in, NULL, 0),
+	SENSOR_ATTR(psoc0_chan1,  S_IRUGO, epm_adc_psoc_show_in, NULL, 1),
+	SENSOR_ATTR(psoc0_chan2,  S_IRUGO, epm_adc_psoc_show_in, NULL, 2),
+	SENSOR_ATTR(psoc0_chan3,  S_IRUGO, epm_adc_psoc_show_in, NULL, 3),
+	SENSOR_ATTR(psoc0_chan4,  S_IRUGO, epm_adc_psoc_show_in, NULL, 4),
+	SENSOR_ATTR(psoc0_chan5,  S_IRUGO, epm_adc_psoc_show_in, NULL, 5),
+	SENSOR_ATTR(psoc0_chan6,  S_IRUGO, epm_adc_psoc_show_in, NULL, 6),
+	SENSOR_ATTR(psoc0_chan7,  S_IRUGO, epm_adc_psoc_show_in, NULL, 7),
+	SENSOR_ATTR(psoc0_chan8,  S_IRUGO, epm_adc_psoc_show_in, NULL, 8),
+	SENSOR_ATTR(psoc0_chan9,  S_IRUGO, epm_adc_psoc_show_in, NULL, 9),
+	SENSOR_ATTR(psoc0_chan10, S_IRUGO, epm_adc_psoc_show_in, NULL, 10),
+	SENSOR_ATTR(psoc0_chan11, S_IRUGO, epm_adc_psoc_show_in, NULL, 11),
+	SENSOR_ATTR(psoc0_chan12, S_IRUGO, epm_adc_psoc_show_in, NULL, 12),
+	SENSOR_ATTR(psoc0_chan13, S_IRUGO, epm_adc_psoc_show_in, NULL, 13),
+	SENSOR_ATTR(psoc0_chan14, S_IRUGO, epm_adc_psoc_show_in, NULL, 14),
+	SENSOR_ATTR(psoc0_chan15, S_IRUGO, epm_adc_psoc_show_in, NULL, 15),
+	SENSOR_ATTR(psoc0_chan16,  S_IRUGO, epm_adc_psoc_show_in, NULL, 16),
+	SENSOR_ATTR(psoc0_chan17,  S_IRUGO, epm_adc_psoc_show_in, NULL, 17),
+	SENSOR_ATTR(psoc0_chan18,  S_IRUGO, epm_adc_psoc_show_in, NULL, 18),
+	SENSOR_ATTR(psoc0_chan19,  S_IRUGO, epm_adc_psoc_show_in, NULL, 19),
+	SENSOR_ATTR(psoc0_chan20,  S_IRUGO, epm_adc_psoc_show_in, NULL, 20),
+	SENSOR_ATTR(psoc0_chan21,  S_IRUGO, epm_adc_psoc_show_in, NULL, 21),
+	SENSOR_ATTR(psoc0_chan22,  S_IRUGO, epm_adc_psoc_show_in, NULL, 22),
+	SENSOR_ATTR(psoc0_chan23,  S_IRUGO, epm_adc_psoc_show_in, NULL, 23),
+	SENSOR_ATTR(psoc0_chan24,  S_IRUGO, epm_adc_psoc_show_in, NULL, 24),
+	SENSOR_ATTR(psoc0_chan25,  S_IRUGO, epm_adc_psoc_show_in, NULL, 25),
+	SENSOR_ATTR(psoc0_chan26, S_IRUGO, epm_adc_psoc_show_in, NULL, 26),
+	SENSOR_ATTR(psoc0_chan27, S_IRUGO, epm_adc_psoc_show_in, NULL, 27),
+	SENSOR_ATTR(psoc0_chan28, S_IRUGO, epm_adc_psoc_show_in, NULL, 28),
+	SENSOR_ATTR(psoc0_chan29, S_IRUGO, epm_adc_psoc_show_in, NULL, 29),
+	SENSOR_ATTR(psoc0_chan30, S_IRUGO, epm_adc_psoc_show_in, NULL, 30),
+	SENSOR_ATTR(psoc0_chan31, S_IRUGO, epm_adc_psoc_show_in, NULL, 31),
 };
 
 static int __devinit epm_adc_psoc_init_hwmon(struct spi_device *spi,
@@ -1677,6 +1692,15 @@
 	epm_adc = epm_adc_drv;
 	epm_adc->misc.name = EPM_ADC_DRIVER_NAME;
 	epm_adc->misc.minor = MISC_DYNAMIC_MINOR;
+
+	if (node) {
+		epm_adc->misc.fops = &epm_adc_fops;
+		if (misc_register(&epm_adc->misc)) {
+			pr_err("Unable to register misc device!\n");
+			return -EFAULT;
+		}
+	}
+
 	epm_adc_drv->epm_spi_client = spi;
 	epm_adc_drv->epm_spi_client->bits_per_word =
 				EPM_ADC_ADS_SPI_BITS_PER_WORD;
diff --git a/drivers/hwmon/qpnp-adc-common.c b/drivers/hwmon/qpnp-adc-common.c
index 5fb041d..440dc42 100644
--- a/drivers/hwmon/qpnp-adc-common.c
+++ b/drivers/hwmon/qpnp-adc-common.c
@@ -632,7 +632,7 @@
 
 	do_div(adc_voltage, param1.dy);
 
-	qpnp_adc_map_temp_voltage(adcmap_100k_104ef_104fb,
+	qpnp_adc_map_voltage_temp(adcmap_100k_104ef_104fb,
 		ARRAY_SIZE(adcmap_100k_104ef_104fb),
 		adc_voltage, result);
 	if (negative_offset)
@@ -649,7 +649,7 @@
 
 	qpnp_get_vadc_gain_and_offset(&param1, CALIB_RATIOMETRIC);
 
-	rc = qpnp_adc_map_voltage_temp(adcmap_100k_104ef_104fb,
+	rc = qpnp_adc_map_temp_voltage(adcmap_100k_104ef_104fb,
 		ARRAY_SIZE(adcmap_100k_104ef_104fb),
 		param->low_thr_temp, &param->low_thr_voltage);
 	if (rc)
@@ -659,7 +659,7 @@
 	do_div(param->low_thr_voltage, param1.adc_vref);
 	param->low_thr_voltage += param1.adc_gnd;
 
-	rc = qpnp_adc_map_voltage_temp(adcmap_100k_104ef_104fb,
+	rc = qpnp_adc_map_temp_voltage(adcmap_100k_104ef_104fb,
 		ARRAY_SIZE(adcmap_100k_104ef_104fb),
 		param->high_thr_temp, &param->high_thr_voltage);
 	if (rc)
@@ -822,7 +822,7 @@
 	struct device_node *node = spmi->dev.of_node;
 	struct resource *res;
 	struct device_node *child;
-	struct qpnp_vadc_amux *adc_channel_list;
+	struct qpnp_adc_amux *adc_channel_list;
 	struct qpnp_adc_properties *adc_prop;
 	struct qpnp_adc_amux_properties *amux_prop;
 	int count_adc_channel_list = 0, decimation, rc = 0, i = 0;
@@ -847,7 +847,7 @@
 		return -ENOMEM;
 	}
 	adc_channel_list = devm_kzalloc(&spmi->dev,
-		((sizeof(struct qpnp_vadc_amux)) * count_adc_channel_list),
+		((sizeof(struct qpnp_adc_amux)) * count_adc_channel_list),
 				GFP_KERNEL);
 	if (!adc_channel_list) {
 		dev_err(&spmi->dev, "Unable to allocate memory\n");
@@ -877,8 +877,7 @@
 			return -EINVAL;
 		}
 
-		rc = of_property_read_u32(child, "qcom,channel-num",
-								&channel_num);
+		rc = of_property_read_u32(child, "reg", &channel_num);
 		if (rc) {
 			pr_err("Invalid channel num\n");
 			return -EINVAL;
diff --git a/drivers/hwmon/qpnp-adc-current.c b/drivers/hwmon/qpnp-adc-current.c
index b5ee104..8071687e 100644
--- a/drivers/hwmon/qpnp-adc-current.c
+++ b/drivers/hwmon/qpnp-adc-current.c
@@ -171,6 +171,47 @@
 	return 0;
 }
 
+static int32_t qpnp_iadc_status_debug(void)
+{
+	int rc = 0;
+	u8 mode = 0, status1 = 0, chan = 0, dig = 0, en = 0;
+
+	rc = qpnp_iadc_read_reg(QPNP_IADC_MODE_CTL, &mode);
+	if (rc < 0) {
+		pr_err("mode ctl register read failed with %d\n", rc);
+		return rc;
+	}
+
+	rc = qpnp_iadc_read_reg(QPNP_ADC_DIG_PARAM, &dig);
+	if (rc < 0) {
+		pr_err("digital param read failed with %d\n", rc);
+		return rc;
+	}
+
+	rc = qpnp_iadc_read_reg(QPNP_IADC_ADC_CH_SEL_CTL, &chan);
+	if (rc < 0) {
+		pr_err("channel read failed with %d\n", rc);
+		return rc;
+	}
+
+	rc = qpnp_iadc_read_reg(QPNP_STATUS1, &status1);
+	if (rc < 0) {
+		pr_err("status1 read failed with %d\n", rc);
+		return rc;
+	}
+
+	rc = qpnp_iadc_read_reg(QPNP_IADC_EN_CTL1, &en);
+	if (rc < 0) {
+		pr_err("en read failed with %d\n", rc);
+		return rc;
+	}
+
+	pr_err("EOC not set with status:%x, dig:%x, ch:%x, mode:%x, en:%x\n",
+			status1, dig, chan, mode, en);
+
+	return 0;
+}
+
 static void trigger_iadc_completion(struct work_struct *work)
 {
 	struct qpnp_iadc_drv *iadc = qpnp_iadc;
@@ -253,7 +294,7 @@
 
 	qpnp_iadc_dig_param_reg |= iadc->adc->amux_prop->decimation <<
 					QPNP_IADC_DEC_RATIO_SEL;
-
+	qpnp_iadc_mode_reg |= QPNP_ADC_TRIM_EN;
 	qpnp_iadc_conv_req = QPNP_IADC_CONV_REQ;
 
 	rc = qpnp_iadc_write_reg(QPNP_IADC_MODE_CTL, qpnp_iadc_mode_reg);
@@ -311,12 +352,15 @@
 		if (status1 == QPNP_STATUS1_EOC)
 			pr_debug("End of conversion status set\n");
 		else {
-			pr_err("EOC interrupt not received\n");
+			rc = qpnp_iadc_status_debug();
+			if (rc < 0) {
+				pr_err("status1 read failed with %d\n", rc);
+				return rc;
+			}
 			return -EINVAL;
 		}
 	}
 
-
 	rc = qpnp_iadc_read_conversion_result(raw_code);
 	if (rc) {
 		pr_err("qpnp adc read adc failed with %d\n", rc);
diff --git a/drivers/hwmon/qpnp-adc-voltage.c b/drivers/hwmon/qpnp-adc-voltage.c
index b71c998..d8d5b53 100644
--- a/drivers/hwmon/qpnp-adc-voltage.c
+++ b/drivers/hwmon/qpnp-adc-voltage.c
@@ -170,7 +170,7 @@
 	return 0;
 }
 
-int32_t qpnp_vadc_configure(
+static int32_t qpnp_vadc_configure(
 			struct qpnp_adc_amux_properties *chan_prop)
 {
 	struct qpnp_vadc_drv *vadc = qpnp_vadc;
@@ -179,7 +179,8 @@
 	int rc = 0;
 
 	/* Mode selection */
-	mode_ctrl = chan_prop->mode_sel << QPNP_VADC_OP_MODE_SHIFT;
+	mode_ctrl |= ((chan_prop->mode_sel << QPNP_VADC_OP_MODE_SHIFT) |
+			(QPNP_VADC_ADC_TRIM_EN | QPNP_VADC_AMUX_TRIM_EN));
 	rc = qpnp_vadc_write_reg(QPNP_VADC_MODE_CTL, mode_ctrl);
 	if (rc < 0) {
 		pr_err("Mode configure write error\n");
@@ -260,7 +261,6 @@
 
 	return 0;
 }
-EXPORT_SYMBOL(qpnp_vadc_configure);
 
 static int32_t qpnp_vadc_read_conversion_result(int32_t *data)
 {
@@ -579,11 +579,11 @@
 
 	vadc->adc->amux_prop->amux_channel = channel;
 
-	while (vadc->adc->adc_channels[dt_index].channel_num
-			!= channel || dt_index > vadc->max_channels_available)
+	while ((vadc->adc->adc_channels[dt_index].channel_num
+		!= channel) && (dt_index < vadc->max_channels_available))
 		dt_index++;
 
-	if (dt_index > vadc->max_channels_available) {
+	if (dt_index >= vadc->max_channels_available) {
 		pr_err("not a valid VADC channel\n");
 		rc = -EINVAL;
 		goto fail_unlock;
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 085b632..92d162b 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2009-2012, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2009-2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -164,7 +164,6 @@
 	struct msm_i2c_platform_data *pdata;
 	int                          suspended;
 	int                          clk_state;
-	struct timer_list            pwr_timer;
 	struct mutex                 mlock;
 	void                         *complete;
 	int                          i2c_gpios[ARRAY_SIZE(i2c_rsrcs)];
@@ -197,6 +196,9 @@
 	uint32_t op_flgs = readl_relaxed(dev->base + QUP_OPERATIONAL);
 	int err = 0;
 
+	if (pm_runtime_suspended(dev->dev))
+		return IRQ_NONE;
+
 	if (!dev->msg || !dev->complete) {
 		/* Clear Error interrupt if it's a level triggered interrupt*/
 		if (dev->num_irqs == 1) {
@@ -327,27 +329,18 @@
 {
 	dev->clk_state = state;
 	if (state != 0) {
-		clk_enable(dev->clk);
+		clk_prepare_enable(dev->clk);
 		if (!dev->pdata->keep_ahb_clk_on)
-			clk_enable(dev->pclk);
+			clk_prepare_enable(dev->pclk);
 	} else {
 		qup_update_state(dev, QUP_RESET_STATE);
-		clk_disable(dev->clk);
+		clk_disable_unprepare(dev->clk);
 		qup_config_core_on_en(dev);
 		if (!dev->pdata->keep_ahb_clk_on)
-			clk_disable(dev->pclk);
+			clk_disable_unprepare(dev->pclk);
 	}
 }
 
-static void
-qup_i2c_pwr_timer(unsigned long data)
-{
-	struct qup_i2c_dev *dev = (struct qup_i2c_dev *) data;
-	dev_dbg(dev->dev, "QUP_Power: Inactivity based power management\n");
-	if (dev->clk_state == 1)
-		qup_i2c_pwr_mgmt(dev, 0);
-}
-
 static int
 qup_i2c_poll_writeready(struct qup_i2c_dev *dev, int rem)
 {
@@ -756,7 +749,7 @@
 	long timeout;
 	int err;
 
-	del_timer_sync(&dev->pwr_timer);
+	pm_runtime_get_sync(dev->dev);
 	mutex_lock(&dev->mlock);
 
 	if (dev->suspended) {
@@ -764,9 +757,6 @@
 		return -EIO;
 	}
 
-	if (dev->clk_state == 0)
-		qup_i2c_pwr_mgmt(dev, 1);
-
 	/* Initialize QUP registers during first transfer */
 	if (dev->clk_ctl == 0) {
 		int fs_div;
@@ -1068,9 +1058,9 @@
 	dev->pos = 0;
 	dev->err = 0;
 	dev->cnt = 0;
-	dev->pwr_timer.expires = jiffies + 3*HZ;
-	add_timer(&dev->pwr_timer);
 	mutex_unlock(&dev->mlock);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 	return ret;
 }
 
@@ -1322,21 +1312,14 @@
 	if (pdata->msm_i2c_config_gpio)
 		pdata->msm_i2c_config_gpio(dev->adapter.nr, 1);
 
-	dev->suspended = 0;
 	mutex_init(&dev->mlock);
 	dev->clk_state = 0;
-	clk_prepare(dev->clk);
-	clk_prepare(dev->pclk);
 	/* If the same AHB clock is used on Modem side
 	 * switch it on here itself and don't switch it
 	 * on and off during suspend and resume.
 	 */
 	if (dev->pdata->keep_ahb_clk_on)
-		clk_enable(dev->pclk);
-	setup_timer(&dev->pwr_timer, qup_i2c_pwr_timer, (unsigned long) dev);
-
-	pm_runtime_set_active(&pdev->dev);
-	pm_runtime_enable(&pdev->dev);
+		clk_prepare_enable(dev->pclk);
 
 	ret = i2c_add_numbered_adapter(&dev->adapter);
 	if (ret) {
@@ -1351,6 +1334,10 @@
 			dev->adapter.dev.of_node = pdev->dev.of_node;
 			of_i2c_register_devices(&dev->adapter);
 		}
+
+		pm_runtime_set_autosuspend_delay(&pdev->dev, MSEC_PER_SEC);
+		pm_runtime_use_autosuspend(&pdev->dev);
+		pm_runtime_enable(&pdev->dev);
 		return 0;
 	}
 
@@ -1393,7 +1380,6 @@
 	dev->suspended = 1;
 	mutex_unlock(&dev->mlock);
 	mutex_destroy(&dev->mlock);
-	del_timer_sync(&dev->pwr_timer);
 	if (dev->clk_state != 0)
 		qup_i2c_pwr_mgmt(dev, 0);
 	platform_set_drvdata(pdev, NULL);
@@ -1403,9 +1389,7 @@
 	}
 	free_irq(dev->err_irq, dev);
 	i2c_del_adapter(&dev->adapter);
-	clk_unprepare(dev->clk);
 	if (!dev->pdata->keep_ahb_clk_on) {
-		clk_unprepare(dev->pclk);
 		clk_put(dev->pclk);
 	}
 	clk_put(dev->clk);
@@ -1415,6 +1399,7 @@
 	iounmap(dev->base);
 
 	pm_runtime_disable(&pdev->dev);
+	pm_runtime_set_suspended(&pdev->dev);
 
 	if (!(dev->pdata->use_gsbi_shared_mode)) {
 		gsbi_mem = platform_get_resource_byname(pdev, IORESOURCE_MEM,
@@ -1431,67 +1416,63 @@
 }
 
 #ifdef CONFIG_PM
-static int qup_i2c_suspend(struct device *device)
+static int i2c_qup_pm_suspend_runtime(struct device *device)
 {
 	struct platform_device *pdev = to_platform_device(device);
 	struct qup_i2c_dev *dev = platform_get_drvdata(pdev);
-
+	dev_dbg(device, "pm_runtime: suspending...\n");
 	/* Grab mutex to ensure ongoing transaction is over */
 	mutex_lock(&dev->mlock);
 	dev->suspended = 1;
 	mutex_unlock(&dev->mlock);
-	del_timer_sync(&dev->pwr_timer);
 	if (dev->clk_state != 0)
 		qup_i2c_pwr_mgmt(dev, 0);
-	clk_unprepare(dev->clk);
-	if (!dev->pdata->keep_ahb_clk_on)
-		clk_unprepare(dev->pclk);
 	qup_i2c_free_gpios(dev);
 	return 0;
 }
 
+static int i2c_qup_pm_resume_runtime(struct device *device)
+{
+	struct platform_device *pdev = to_platform_device(device);
+	struct qup_i2c_dev *dev = platform_get_drvdata(pdev);
+	dev_dbg(device, "pm_runtime: resuming...\n");
+	BUG_ON(qup_i2c_request_gpios(dev) != 0);
+	if (dev->clk_state == 0)
+		qup_i2c_pwr_mgmt(dev, 1);
+	dev->suspended = 0;
+	return 0;
+}
+
+static int qup_i2c_suspend(struct device *device)
+{
+	if (!pm_runtime_enabled(device) || !pm_runtime_suspended(device)) {
+		dev_dbg(device, "system suspend");
+		i2c_qup_pm_suspend_runtime(device);
+	}
+	return 0;
+}
+
 static int qup_i2c_resume(struct device *device)
 {
-	struct platform_device *pdev = to_platform_device(device);
-	struct qup_i2c_dev *dev = platform_get_drvdata(pdev);
-	BUG_ON(qup_i2c_request_gpios(dev) != 0);
-	clk_prepare(dev->clk);
-	if (!dev->pdata->keep_ahb_clk_on)
-		clk_prepare(dev->pclk);
-	dev->suspended = 0;
+	if (!pm_runtime_enabled(device) || !pm_runtime_suspended(device)) {
+		dev_dbg(device, "system resume");
+		i2c_qup_pm_resume_runtime(device);
+		pm_runtime_mark_last_busy(device);
+		pm_request_autosuspend(device);
+	}
 	return 0;
 }
 #endif /* CONFIG_PM */
 
-#ifdef CONFIG_PM_RUNTIME
-static int i2c_qup_runtime_idle(struct device *dev)
-{
-	dev_dbg(dev, "pm_runtime: idle...\n");
-	return 0;
-}
-
-static int i2c_qup_runtime_suspend(struct device *dev)
-{
-	dev_dbg(dev, "pm_runtime: suspending...\n");
-	return 0;
-}
-
-static int i2c_qup_runtime_resume(struct device *dev)
-{
-	dev_dbg(dev, "pm_runtime: resuming...\n");
-	return 0;
-}
-#endif
-
 static const struct dev_pm_ops i2c_qup_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(
 		qup_i2c_suspend,
 		qup_i2c_resume
 	)
 	SET_RUNTIME_PM_OPS(
-		i2c_qup_runtime_suspend,
-		i2c_qup_runtime_resume,
-		i2c_qup_runtime_idle
+		i2c_qup_pm_suspend_runtime,
+		i2c_qup_pm_resume_runtime,
+		NULL
 	)
 };
 
diff --git a/drivers/input/misc/lis3dh_acc.c b/drivers/input/misc/lis3dh_acc.c
index cc4ee9f..ea1b079 100644
--- a/drivers/input/misc/lis3dh_acc.c
+++ b/drivers/input/misc/lis3dh_acc.c
@@ -274,6 +274,7 @@
 				rc = PTR_ERR(lis3dh_acc_vreg[i].vreg);
 				pr_err("%s:regulator get failed rc=%d\n",
 								__func__, rc);
+				lis3dh_acc_vreg[i].vreg = NULL;
 				goto error_vdd;
 			}
 
@@ -287,6 +288,7 @@
 					pr_err("%s: set voltage failed rc=%d\n",
 					__func__, rc);
 					regulator_put(lis3dh_acc_vreg[i].vreg);
+					lis3dh_acc_vreg[i].vreg = NULL;
 					goto error_vdd;
 				}
 			}
@@ -302,6 +304,7 @@
 						lis3dh_acc_vreg[i].max_uV);
 				}
 				regulator_put(lis3dh_acc_vreg[i].vreg);
+				lis3dh_acc_vreg[i].vreg = NULL;
 				goto error_vdd;
 			}
 		}
@@ -312,12 +315,16 @@
 
 error_vdd:
 	while (--i >= 0) {
-		if (regulator_count_voltages(lis3dh_acc_vreg[i].vreg) > 0) {
-			regulator_set_voltage(lis3dh_acc_vreg[i].vreg, 0,
-						lis3dh_acc_vreg[i].max_uV);
+		if (!IS_ERR_OR_NULL(lis3dh_acc_vreg[i].vreg)) {
+			if (regulator_count_voltages(
+			lis3dh_acc_vreg[i].vreg) > 0) {
+				regulator_set_voltage(lis3dh_acc_vreg[i].vreg,
+						0, lis3dh_acc_vreg[i].max_uV);
+			}
+			regulator_disable(lis3dh_acc_vreg[i].vreg);
+			regulator_put(lis3dh_acc_vreg[i].vreg);
+			lis3dh_acc_vreg[i].vreg = NULL;
 		}
-		regulator_disable(lis3dh_acc_vreg[i].vreg);
-		regulator_put(lis3dh_acc_vreg[i].vreg);
 	}
 	return rc;
 }
diff --git a/drivers/input/misc/mpu3050.c b/drivers/input/misc/mpu3050.c
index db6f93c..6c64a57 100644
--- a/drivers/input/misc/mpu3050.c
+++ b/drivers/input/misc/mpu3050.c
@@ -184,6 +184,7 @@
 				rc = PTR_ERR(mpu_vreg[i].vreg);
 				pr_err("%s:regulator get failed rc=%d\n",
 						__func__, rc);
+				mpu_vreg[i].vreg = NULL;
 				goto error_vdd;
 			}
 
@@ -194,6 +195,7 @@
 					pr_err("%s:set_voltage failed rc=%d\n",
 						__func__, rc);
 					regulator_put(mpu_vreg[i].vreg);
+					mpu_vreg[i].vreg = NULL;
 					goto error_vdd;
 				}
 			}
@@ -210,6 +212,7 @@
 						0, mpu_vreg[i].max_uV);
 				}
 				regulator_put(mpu_vreg[i].vreg);
+				mpu_vreg[i].vreg = NULL;
 				goto error_vdd;
 			}
 		}
@@ -219,12 +222,16 @@
 	}
 error_vdd:
 	while (--i >= 0) {
-		if (regulator_count_voltages(mpu_vreg[i].vreg) > 0) {
-			regulator_set_voltage(mpu_vreg[i].vreg, 0,
+		if (!IS_ERR_OR_NULL(mpu_vreg[i].vreg)) {
+			if (regulator_count_voltages(
+				mpu_vreg[i].vreg) > 0) {
+				regulator_set_voltage(mpu_vreg[i].vreg, 0,
 						mpu_vreg[i].max_uV);
+			}
+			regulator_disable(mpu_vreg[i].vreg);
+			regulator_put(mpu_vreg[i].vreg);
+			mpu_vreg[i].vreg = NULL;
 		}
-		regulator_disable(mpu_vreg[i].vreg);
-		regulator_put(mpu_vreg[i].vreg);
 	}
 	return rc;
 }
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index b3bd8a0..bb505d8 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -1820,6 +1820,7 @@
 		kfree(data->object_table);
 		data->object_table = NULL;
 		data->cfg_version_idx = 0;
+		data->config_info = NULL;
 		data->update_cfg = false;
 
 		/* T38 object address might have changed, read it from
diff --git a/drivers/input/touchscreen/cyttsp-i2c-qc.c b/drivers/input/touchscreen/cyttsp-i2c-qc.c
index f96348e..6eba5d1 100644
--- a/drivers/input/touchscreen/cyttsp-i2c-qc.c
+++ b/drivers/input/touchscreen/cyttsp-i2c-qc.c
@@ -2778,11 +2778,14 @@
 			return -EINVAL;
 		}
 
+		mutex_init(&ts->mutex);
 		i2c_set_clientdata(client, ts);
 
 		error = cyttsp_initialize(client, ts);
 		if (error) {
 			cyttsp_xdebug1("err cyttsp_initialize\n");
+			/* release mutex */
+			mutex_destroy(&ts->mutex);
 			/* deallocate memory */
 			kfree(ts);
 /*
@@ -2801,7 +2804,6 @@
 	}
 #endif /* CONFIG_HAS_EARLYSUSPEND */
 	device_init_wakeup(&client->dev, ts->platform_data->wakeup);
-	mutex_init(&ts->mutex);
 
 	cyttsp_info("Start Probe %s\n", \
 		(retval < CY_OK) ? "FAIL" : "PASS");
diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index 332138c..86703e0 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -16,7 +16,7 @@
 # MSM IOMMU support
 config MSM_IOMMU
 	bool "MSM IOMMU Support"
-	depends on ARCH_MSM8X60 || ARCH_MSM8960 || ARCH_APQ8064 || ARCH_MSM8974 || ARCH_MPQ8092
+	depends on ARCH_MSM8X60 || ARCH_MSM8960 || ARCH_APQ8064 || ARCH_MSM8974 || ARCH_MPQ8092 || ARCH_MSM8910
 	select IOMMU_API
 	help
 	  Support for the IOMMUs found on certain Qualcomm SOCs.
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index a5bee8e..32c00cd 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3193,26 +3193,6 @@
 	return 0;
 }
 
-static int amd_iommu_device_group(struct device *dev, unsigned int *groupid)
-{
-	struct iommu_dev_data *dev_data = dev->archdata.iommu;
-	struct pci_dev *pdev = to_pci_dev(dev);
-	u16 devid;
-
-	if (!dev_data)
-		return -ENODEV;
-
-	if (pdev->is_virtfn || !iommu_group_mf)
-		devid = dev_data->devid;
-	else
-		devid = calc_devid(pdev->bus->number,
-				   PCI_DEVFN(PCI_SLOT(pdev->devfn), 0));
-
-	*groupid = amd_iommu_alias_table[devid];
-
-	return 0;
-}
-
 static struct iommu_ops amd_iommu_ops = {
 	.domain_init = amd_iommu_domain_init,
 	.domain_destroy = amd_iommu_domain_destroy,
@@ -3222,7 +3202,6 @@
 	.unmap = amd_iommu_unmap,
 	.iova_to_phys = amd_iommu_iova_to_phys,
 	.domain_has_cap = amd_iommu_domain_has_cap,
-	.device_group = amd_iommu_device_group,
 	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index f93d5ac..d4a0ff7 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -4087,54 +4087,6 @@
 	return 0;
 }
 
-/*
- * Group numbers are arbitrary.  Device with the same group number
- * indicate the iommu cannot differentiate between them.  To avoid
- * tracking used groups we just use the seg|bus|devfn of the lowest
- * level we're able to differentiate devices
- */
-static int intel_iommu_device_group(struct device *dev, unsigned int *groupid)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct pci_dev *bridge;
-	union {
-		struct {
-			u8 devfn;
-			u8 bus;
-			u16 segment;
-		} pci;
-		u32 group;
-	} id;
-
-	if (iommu_no_mapping(dev))
-		return -ENODEV;
-
-	id.pci.segment = pci_domain_nr(pdev->bus);
-	id.pci.bus = pdev->bus->number;
-	id.pci.devfn = pdev->devfn;
-
-	if (!device_to_iommu(id.pci.segment, id.pci.bus, id.pci.devfn))
-		return -ENODEV;
-
-	bridge = pci_find_upstream_pcie_bridge(pdev);
-	if (bridge) {
-		if (pci_is_pcie(bridge)) {
-			id.pci.bus = bridge->subordinate->number;
-			id.pci.devfn = 0;
-		} else {
-			id.pci.bus = bridge->bus->number;
-			id.pci.devfn = bridge->devfn;
-		}
-	}
-
-	if (!pdev->is_virtfn && iommu_group_mf)
-		id.pci.devfn = PCI_DEVFN(PCI_SLOT(id.pci.devfn), 0);
-
-	*groupid = id.group;
-
-	return 0;
-}
-
 static struct iommu_ops intel_iommu_ops = {
 	.domain_init	= intel_iommu_domain_init,
 	.domain_destroy = intel_iommu_domain_destroy,
@@ -4144,7 +4096,6 @@
 	.unmap		= intel_iommu_unmap,
 	.iova_to_phys	= intel_iommu_iova_to_phys,
 	.domain_has_cap = intel_iommu_domain_has_cap,
-	.device_group	= intel_iommu_device_group,
 	.pgsize_bitmap	= INTEL_IOMMU_PGSIZES,
 };
 
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index ef69d91..d07fb96 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -27,60 +27,535 @@
 #include <linux/errno.h>
 #include <linux/iommu.h>
 #include <linux/scatterlist.h>
+#include <linux/idr.h>
+#include <linux/notifier.h>
+#include <linux/err.h>
 
-static ssize_t show_iommu_group(struct device *dev,
-				struct device_attribute *attr, char *buf)
+static struct kset *iommu_group_kset;
+static struct ida iommu_group_ida;
+static struct mutex iommu_group_mutex;
+
+struct iommu_group {
+	struct kobject kobj;
+	struct kobject *devices_kobj;
+	struct list_head devices;
+	struct mutex mutex;
+	struct blocking_notifier_head notifier;
+	void *iommu_data;
+	void (*iommu_data_release)(void *iommu_data);
+	char *name;
+	int id;
+};
+
+struct iommu_device {
+	struct list_head list;
+	struct device *dev;
+	char *name;
+};
+
+struct iommu_group_attribute {
+	struct attribute attr;
+	ssize_t (*show)(struct iommu_group *group, char *buf);
+	ssize_t (*store)(struct iommu_group *group,
+			 const char *buf, size_t count);
+};
+
+#define IOMMU_GROUP_ATTR(_name, _mode, _show, _store)		\
+struct iommu_group_attribute iommu_group_attr_##_name =		\
+	__ATTR(_name, _mode, _show, _store)
+
+#define to_iommu_group_attr(_attr)	\
+	container_of(_attr, struct iommu_group_attribute, attr)
+#define to_iommu_group(_kobj)		\
+	container_of(_kobj, struct iommu_group, kobj)
+
+static ssize_t iommu_group_attr_show(struct kobject *kobj,
+				     struct attribute *__attr, char *buf)
 {
-	unsigned int groupid;
+	struct iommu_group_attribute *attr = to_iommu_group_attr(__attr);
+	struct iommu_group *group = to_iommu_group(kobj);
+	ssize_t ret = -EIO;
 
-	if (iommu_device_group(dev, &groupid))
-		return 0;
-
-	return sprintf(buf, "%u", groupid);
+	if (attr->show)
+		ret = attr->show(group, buf);
+	return ret;
 }
-static DEVICE_ATTR(iommu_group, S_IRUGO, show_iommu_group, NULL);
+
+static ssize_t iommu_group_attr_store(struct kobject *kobj,
+				      struct attribute *__attr,
+				      const char *buf, size_t count)
+{
+	struct iommu_group_attribute *attr = to_iommu_group_attr(__attr);
+	struct iommu_group *group = to_iommu_group(kobj);
+	ssize_t ret = -EIO;
+
+	if (attr->store)
+		ret = attr->store(group, buf, count);
+	return ret;
+}
+
+static const struct sysfs_ops iommu_group_sysfs_ops = {
+	.show = iommu_group_attr_show,
+	.store = iommu_group_attr_store,
+};
+
+static int iommu_group_create_file(struct iommu_group *group,
+				   struct iommu_group_attribute *attr)
+{
+	return sysfs_create_file(&group->kobj, &attr->attr);
+}
+
+static void iommu_group_remove_file(struct iommu_group *group,
+				    struct iommu_group_attribute *attr)
+{
+	sysfs_remove_file(&group->kobj, &attr->attr);
+}
+
+static ssize_t iommu_group_show_name(struct iommu_group *group, char *buf)
+{
+	return sprintf(buf, "%s\n", group->name);
+}
+
+static IOMMU_GROUP_ATTR(name, S_IRUGO, iommu_group_show_name, NULL);
+
+static void iommu_group_release(struct kobject *kobj)
+{
+	struct iommu_group *group = to_iommu_group(kobj);
+
+	if (group->iommu_data_release)
+		group->iommu_data_release(group->iommu_data);
+
+	mutex_lock(&iommu_group_mutex);
+	ida_remove(&iommu_group_ida, group->id);
+	mutex_unlock(&iommu_group_mutex);
+
+	kfree(group->name);
+	kfree(group);
+}
+
+static struct kobj_type iommu_group_ktype = {
+	.sysfs_ops = &iommu_group_sysfs_ops,
+	.release = iommu_group_release,
+};
+
+/**
+ * iommu_group_alloc - Allocate a new group
+ * @name: Optional name to associate with group, visible in sysfs
+ *
+ * This function is called by an iommu driver to allocate a new iommu
+ * group.  The iommu group represents the minimum granularity of the iommu.
+ * Upon successful return, the caller holds a reference to the supplied
+ * group in order to hold the group until devices are added.  Use
+ * iommu_group_put() to release this extra reference count, allowing the
+ * group to be automatically reclaimed once it has no devices or external
+ * references.
+ */
+struct iommu_group *iommu_group_alloc(void)
+{
+	struct iommu_group *group;
+	int ret;
+
+	group = kzalloc(sizeof(*group), GFP_KERNEL);
+	if (!group)
+		return ERR_PTR(-ENOMEM);
+
+	group->kobj.kset = iommu_group_kset;
+	mutex_init(&group->mutex);
+	INIT_LIST_HEAD(&group->devices);
+	BLOCKING_INIT_NOTIFIER_HEAD(&group->notifier);
+
+	mutex_lock(&iommu_group_mutex);
+
+again:
+	if (unlikely(0 == ida_pre_get(&iommu_group_ida, GFP_KERNEL))) {
+		kfree(group);
+		mutex_unlock(&iommu_group_mutex);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	if (-EAGAIN == ida_get_new(&iommu_group_ida, &group->id))
+		goto again;
+
+	mutex_unlock(&iommu_group_mutex);
+
+	ret = kobject_init_and_add(&group->kobj, &iommu_group_ktype,
+				   NULL, "%d", group->id);
+	if (ret) {
+		mutex_lock(&iommu_group_mutex);
+		ida_remove(&iommu_group_ida, group->id);
+		mutex_unlock(&iommu_group_mutex);
+		kfree(group);
+		return ERR_PTR(ret);
+	}
+
+	group->devices_kobj = kobject_create_and_add("devices", &group->kobj);
+	if (!group->devices_kobj) {
+		kobject_put(&group->kobj); /* triggers .release & free */
+		return ERR_PTR(-ENOMEM);
+	}
+
+	/*
+	 * The devices_kobj holds a reference on the group kobject, so
+	 * as long as that exists so will the group.  We can therefore
+	 * use the devices_kobj for reference counting.
+	 */
+	kobject_put(&group->kobj);
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(iommu_group_alloc);
+
+/**
+ * iommu_group_get_iommudata - retrieve iommu_data registered for a group
+ * @group: the group
+ *
+ * iommu drivers can store data in the group for use when doing iommu
+ * operations.  This function provides a way to retrieve it.  Caller
+ * should hold a group reference.
+ */
+void *iommu_group_get_iommudata(struct iommu_group *group)
+{
+	return group->iommu_data;
+}
+EXPORT_SYMBOL_GPL(iommu_group_get_iommudata);
+
+/**
+ * iommu_group_set_iommudata - set iommu_data for a group
+ * @group: the group
+ * @iommu_data: new data
+ * @release: release function for iommu_data
+ *
+ * iommu drivers can store data in the group for use when doing iommu
+ * operations.  This function provides a way to set the data after
+ * the group has been allocated.  Caller should hold a group reference.
+ */
+void iommu_group_set_iommudata(struct iommu_group *group, void *iommu_data,
+			       void (*release)(void *iommu_data))
+{
+	group->iommu_data = iommu_data;
+	group->iommu_data_release = release;
+}
+EXPORT_SYMBOL_GPL(iommu_group_set_iommudata);
+
+/**
+ * iommu_group_set_name - set name for a group
+ * @group: the group
+ * @name: name
+ *
+ * Allow iommu driver to set a name for a group.  When set it will
+ * appear in a name attribute file under the group in sysfs.
+ */
+int iommu_group_set_name(struct iommu_group *group, const char *name)
+{
+	int ret;
+
+	if (group->name) {
+		iommu_group_remove_file(group, &iommu_group_attr_name);
+		kfree(group->name);
+		group->name = NULL;
+		if (!name)
+			return 0;
+	}
+
+	group->name = kstrdup(name, GFP_KERNEL);
+	if (!group->name)
+		return -ENOMEM;
+
+	ret = iommu_group_create_file(group, &iommu_group_attr_name);
+	if (ret) {
+		kfree(group->name);
+		group->name = NULL;
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_group_set_name);
+
+/**
+ * iommu_group_add_device - add a device to an iommu group
+ * @group: the group into which to add the device (reference should be held)
+ * @dev: the device
+ *
+ * This function is called by an iommu driver to add a device into a
+ * group.  Adding a device increments the group reference count.
+ */
+int iommu_group_add_device(struct iommu_group *group, struct device *dev)
+{
+	int ret, i = 0;
+	struct iommu_device *device;
+
+	device = kzalloc(sizeof(*device), GFP_KERNEL);
+	if (!device)
+		return -ENOMEM;
+
+	device->dev = dev;
+
+	ret = sysfs_create_link(&dev->kobj, &group->kobj, "iommu_group");
+	if (ret) {
+		kfree(device);
+		return ret;
+	}
+
+	device->name = kasprintf(GFP_KERNEL, "%s", kobject_name(&dev->kobj));
+rename:
+	if (!device->name) {
+		sysfs_remove_link(&dev->kobj, "iommu_group");
+		kfree(device);
+		return -ENOMEM;
+	}
+
+	ret = sysfs_create_link_nowarn(group->devices_kobj,
+				       &dev->kobj, device->name);
+	if (ret) {
+		kfree(device->name);
+		if (ret == -EEXIST && i >= 0) {
+			/*
+			 * Account for the slim chance of collision
+			 * and append an instance to the name.
+			 */
+			device->name = kasprintf(GFP_KERNEL, "%s.%d",
+						 kobject_name(&dev->kobj), i++);
+			goto rename;
+		}
+
+		sysfs_remove_link(&dev->kobj, "iommu_group");
+		kfree(device);
+		return ret;
+	}
+
+	kobject_get(group->devices_kobj);
+
+	dev->iommu_group = group;
+
+	mutex_lock(&group->mutex);
+	list_add_tail(&device->list, &group->devices);
+	mutex_unlock(&group->mutex);
+
+	/* Notify any listeners about change to group. */
+	blocking_notifier_call_chain(&group->notifier,
+				     IOMMU_GROUP_NOTIFY_ADD_DEVICE, dev);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(iommu_group_add_device);
+
+/**
+ * iommu_group_remove_device - remove a device from it's current group
+ * @dev: device to be removed
+ *
+ * This function is called by an iommu driver to remove the device from
+ * it's current group.  This decrements the iommu group reference count.
+ */
+void iommu_group_remove_device(struct device *dev)
+{
+	struct iommu_group *group = dev->iommu_group;
+	struct iommu_device *tmp_device, *device = NULL;
+
+	/* Pre-notify listeners that a device is being removed. */
+	blocking_notifier_call_chain(&group->notifier,
+				     IOMMU_GROUP_NOTIFY_DEL_DEVICE, dev);
+
+	mutex_lock(&group->mutex);
+	list_for_each_entry(tmp_device, &group->devices, list) {
+		if (tmp_device->dev == dev) {
+			device = tmp_device;
+			list_del(&device->list);
+			break;
+		}
+	}
+	mutex_unlock(&group->mutex);
+
+	if (!device)
+		return;
+
+	sysfs_remove_link(group->devices_kobj, device->name);
+	sysfs_remove_link(&dev->kobj, "iommu_group");
+
+	kfree(device->name);
+	kfree(device);
+	dev->iommu_group = NULL;
+	kobject_put(group->devices_kobj);
+}
+EXPORT_SYMBOL_GPL(iommu_group_remove_device);
+
+/**
+ * iommu_group_for_each_dev - iterate over each device in the group
+ * @group: the group
+ * @data: caller opaque data to be passed to callback function
+ * @fn: caller supplied callback function
+ *
+ * This function is called by group users to iterate over group devices.
+ * Callers should hold a reference count to the group during callback.
+ * The group->mutex is held across callbacks, which will block calls to
+ * iommu_group_add/remove_device.
+ */
+int iommu_group_for_each_dev(struct iommu_group *group, void *data,
+			     int (*fn)(struct device *, void *))
+{
+	struct iommu_device *device;
+	int ret = 0;
+
+	mutex_lock(&group->mutex);
+	list_for_each_entry(device, &group->devices, list) {
+		ret = fn(device->dev, data);
+		if (ret)
+			break;
+	}
+	mutex_unlock(&group->mutex);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(iommu_group_for_each_dev);
+
+/**
+ * iommu_group_get - Return the group for a device and increment reference
+ * @dev: get the group that this device belongs to
+ *
+ * This function is called by iommu drivers and users to get the group
+ * for the specified device.  If found, the group is returned and the group
+ * reference in incremented, else NULL.
+ */
+struct iommu_group *iommu_group_get(struct device *dev)
+{
+	struct iommu_group *group = dev->iommu_group;
+
+	if (group)
+		kobject_get(group->devices_kobj);
+
+	return group;
+}
+EXPORT_SYMBOL_GPL(iommu_group_get);
+
+/**
+ * iommu_group_put - Decrement group reference
+ * @group: the group to use
+ *
+ * This function is called by iommu drivers and users to release the
+ * iommu group.  Once the reference count is zero, the group is released.
+ */
+void iommu_group_put(struct iommu_group *group)
+{
+	if (group)
+		kobject_put(group->devices_kobj);
+}
+EXPORT_SYMBOL_GPL(iommu_group_put);
+
+/**
+ * iommu_group_register_notifier - Register a notifier for group changes
+ * @group: the group to watch
+ * @nb: notifier block to signal
+ *
+ * This function allows iommu group users to track changes in a group.
+ * See include/linux/iommu.h for actions sent via this notifier.  Caller
+ * should hold a reference to the group throughout notifier registration.
+ */
+int iommu_group_register_notifier(struct iommu_group *group,
+				  struct notifier_block *nb)
+{
+	return blocking_notifier_chain_register(&group->notifier, nb);
+}
+EXPORT_SYMBOL_GPL(iommu_group_register_notifier);
+
+/**
+ * iommu_group_unregister_notifier - Unregister a notifier
+ * @group: the group to watch
+ * @nb: notifier block to signal
+ *
+ * Unregister a previously registered group notifier block.
+ */
+int iommu_group_unregister_notifier(struct iommu_group *group,
+				    struct notifier_block *nb)
+{
+	return blocking_notifier_chain_unregister(&group->notifier, nb);
+}
+EXPORT_SYMBOL_GPL(iommu_group_unregister_notifier);
+
+/**
+ * iommu_group_id - Return ID for a group
+ * @group: the group to ID
+ *
+ * Return the unique ID for the group matching the sysfs group number.
+ */
+int iommu_group_id(struct iommu_group *group)
+{
+	return group->id;
+}
+EXPORT_SYMBOL_GPL(iommu_group_id);
 
 static int add_iommu_group(struct device *dev, void *data)
 {
-	unsigned int groupid;
+	struct iommu_ops *ops = data;
 
-	if (iommu_device_group(dev, &groupid) == 0)
-		return device_create_file(dev, &dev_attr_iommu_group);
+	if (!ops->add_device)
+		return -ENODEV;
+
+	WARN_ON(dev->iommu_group);
+
+	ops->add_device(dev);
 
 	return 0;
 }
 
-static int remove_iommu_group(struct device *dev)
-{
-	unsigned int groupid;
-
-	if (iommu_device_group(dev, &groupid) == 0)
-		device_remove_file(dev, &dev_attr_iommu_group);
-
-	return 0;
-}
-
-static int iommu_device_notifier(struct notifier_block *nb,
-				 unsigned long action, void *data)
+static int iommu_bus_notifier(struct notifier_block *nb,
+			      unsigned long action, void *data)
 {
 	struct device *dev = data;
+	struct iommu_ops *ops = dev->bus->iommu_ops;
+	struct iommu_group *group;
+	unsigned long group_action = 0;
 
-	if (action == BUS_NOTIFY_ADD_DEVICE)
-		return add_iommu_group(dev, NULL);
-	else if (action == BUS_NOTIFY_DEL_DEVICE)
-		return remove_iommu_group(dev);
+	/*
+	 * ADD/DEL call into iommu driver ops if provided, which may
+	 * result in ADD/DEL notifiers to group->notifier
+	 */
+	if (action == BUS_NOTIFY_ADD_DEVICE) {
+		if (ops->add_device)
+			return ops->add_device(dev);
+	} else if (action == BUS_NOTIFY_DEL_DEVICE) {
+		if (ops->remove_device && dev->iommu_group) {
+			ops->remove_device(dev);
+			return 0;
+		}
+	}
 
+	/*
+	 * Remaining BUS_NOTIFYs get filtered and republished to the
+	 * group, if anyone is listening
+	 */
+	group = iommu_group_get(dev);
+	if (!group)
+		return 0;
+
+	switch (action) {
+	case BUS_NOTIFY_BIND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_BIND_DRIVER;
+		break;
+	case BUS_NOTIFY_BOUND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_BOUND_DRIVER;
+		break;
+	case BUS_NOTIFY_UNBIND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_UNBIND_DRIVER;
+		break;
+	case BUS_NOTIFY_UNBOUND_DRIVER:
+		group_action = IOMMU_GROUP_NOTIFY_UNBOUND_DRIVER;
+		break;
+	}
+
+	if (group_action)
+		blocking_notifier_call_chain(&group->notifier,
+					     group_action, dev);
+
+	iommu_group_put(group);
 	return 0;
 }
 
-static struct notifier_block iommu_device_nb = {
-	.notifier_call = iommu_device_notifier,
+static struct notifier_block iommu_bus_nb = {
+	.notifier_call = iommu_bus_notifier,
 };
 
 static void iommu_bus_init(struct bus_type *bus, struct iommu_ops *ops)
 {
-	bus_register_notifier(bus, &iommu_device_nb);
-	bus_for_each_dev(bus, NULL, NULL, add_iommu_group);
+	bus_register_notifier(bus, &iommu_bus_nb);
+	bus_for_each_dev(bus, NULL, ops, add_iommu_group);
 }
 
 /**
@@ -120,6 +595,7 @@
  * iommu_set_fault_handler() - set a fault handler for an iommu domain
  * @domain: iommu domain
  * @handler: fault handler
+ * @token: user data, will be passed back to the fault handler
  *
  * This function should be used by IOMMU users which want to be notified
  * whenever an IOMMU fault happens.
@@ -128,11 +604,13 @@
  * error code otherwise.
  */
 void iommu_set_fault_handler(struct iommu_domain *domain,
-					iommu_fault_handler_t handler)
+					iommu_fault_handler_t handler,
+					void *token)
 {
 	BUG_ON(!domain);
 
 	domain->handler = handler;
+	domain->handler_token = token;
 }
 EXPORT_SYMBOL_GPL(iommu_set_fault_handler);
 
@@ -190,6 +668,45 @@
 }
 EXPORT_SYMBOL_GPL(iommu_detach_device);
 
+/*
+ * IOMMU groups are really the natrual working unit of the IOMMU, but
+ * the IOMMU API works on domains and devices.  Bridge that gap by
+ * iterating over the devices in a group.  Ideally we'd have a single
+ * device which represents the requestor ID of the group, but we also
+ * allow IOMMU drivers to create policy defined minimum sets, where
+ * the physical hardware may be able to distiguish members, but we
+ * wish to group them at a higher level (ex. untrusted multi-function
+ * PCI devices).  Thus we attach each device.
+ */
+static int iommu_group_do_attach_device(struct device *dev, void *data)
+{
+	struct iommu_domain *domain = data;
+
+	return iommu_attach_device(domain, dev);
+}
+
+int iommu_attach_group(struct iommu_domain *domain, struct iommu_group *group)
+{
+	return iommu_group_for_each_dev(group, domain,
+					iommu_group_do_attach_device);
+}
+EXPORT_SYMBOL_GPL(iommu_attach_group);
+
+static int iommu_group_do_detach_device(struct device *dev, void *data)
+{
+	struct iommu_domain *domain = data;
+
+	iommu_detach_device(domain, dev);
+
+	return 0;
+}
+
+void iommu_detach_group(struct iommu_domain *domain, struct iommu_group *group)
+{
+	iommu_group_for_each_dev(group, domain, iommu_group_do_detach_device);
+}
+EXPORT_SYMBOL_GPL(iommu_detach_group);
+
 phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain,
 			       unsigned long iova)
 {
@@ -367,11 +884,15 @@
 }
 EXPORT_SYMBOL_GPL(iommu_get_pt_base_addr);
 
-int iommu_device_group(struct device *dev, unsigned int *groupid)
+static int __init iommu_init(void)
 {
-	if (iommu_present(dev->bus) && dev->bus->iommu_ops->device_group)
-		return dev->bus->iommu_ops->device_group(dev, groupid);
+	iommu_group_kset = kset_create_and_add("iommu_groups",
+					       NULL, kernel_kobj);
+	ida_init(&iommu_group_ida);
+	mutex_init(&iommu_group_mutex);
 
-	return -ENODEV;
+	BUG_ON(!iommu_group_kset);
+
+	return 0;
 }
-EXPORT_SYMBOL_GPL(iommu_device_group);
+subsys_initcall(iommu_init);
diff --git a/drivers/iommu/msm_iommu-v2.c b/drivers/iommu/msm_iommu-v2.c
index 425eb8a..567b9ba 100644
--- a/drivers/iommu/msm_iommu-v2.c
+++ b/drivers/iommu/msm_iommu-v2.c
@@ -156,9 +156,7 @@
 	int i, smt_size;
 
 	SET_ACR(base, 0);
-	SET_NSACR(base, 0);
 	SET_CR2(base, 0);
-	SET_NSCR2(base, 0);
 	SET_GFAR(base, 0);
 	SET_GFSRRESTORE(base, 0);
 	SET_TLBIALLNSNH(base, 0);
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 4c72df7..1ce1cf8 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -233,13 +233,13 @@
 	return ret;
 }
 
-static void __reset_context(void __iomem *base, int ctx)
+static void __reset_context(void __iomem *base, void __iomem *glb_base, int ctx)
 {
-	SET_BPRCOSH(base, ctx, 0);
-	SET_BPRCISH(base, ctx, 0);
-	SET_BPRCNSH(base, ctx, 0);
-	SET_BPSHCFG(base, ctx, 0);
-	SET_BPMTCFG(base, ctx, 0);
+	SET_BPRCOSH(glb_base, ctx, 0);
+	SET_BPRCISH(glb_base, ctx, 0);
+	SET_BPRCNSH(glb_base, ctx, 0);
+	SET_BPSHCFG(glb_base, ctx, 0);
+	SET_BPMTCFG(glb_base, ctx, 0);
 	SET_ACTLR(base, ctx, 0);
 	SET_SCTLR(base, ctx, 0);
 	SET_FSRRESTORE(base, ctx, 0);
@@ -257,16 +257,15 @@
 	mb();
 }
 
-static void __program_context(void __iomem *base, int ctx, int ncb,
-			      phys_addr_t pgtable, int redirect,
-			      int ttbr_split)
+static void __program_context(void __iomem *base, void __iomem *glb_base,
+			      int ctx, int ncb, phys_addr_t pgtable,
+			      int redirect, int ttbr_split)
 {
 	unsigned int prrr, nmrr;
 	int i, j, found;
-
 	msm_iommu_remote_spin_lock();
 
-	__reset_context(base, ctx);
+	__reset_context(base, glb_base, ctx);
 
 	/* Set up HTW mode */
 	/* TLB miss configuration: perform HTW on miss */
@@ -418,7 +417,6 @@
 static int msm_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
 {
 	struct msm_priv *priv;
-	struct msm_iommu_ctx_dev *ctx_dev;
 	struct msm_iommu_drvdata *iommu_drvdata;
 	struct msm_iommu_ctx_drvdata *ctx_drvdata;
 	struct msm_iommu_ctx_drvdata *tmp_drvdata;
@@ -435,9 +433,8 @@
 
 	iommu_drvdata = dev_get_drvdata(dev->parent);
 	ctx_drvdata = dev_get_drvdata(dev);
-	ctx_dev = dev->platform_data;
 
-	if (!iommu_drvdata || !ctx_drvdata || !ctx_dev) {
+	if (!iommu_drvdata || !ctx_drvdata) {
 		ret = -EINVAL;
 		goto fail;
 	}
@@ -457,7 +454,8 @@
 	if (ret)
 		goto fail;
 
-	__program_context(iommu_drvdata->base, ctx_dev->num, iommu_drvdata->ncb,
+	__program_context(iommu_drvdata->base, iommu_drvdata->glb_base,
+			  ctx_drvdata->num, iommu_drvdata->ncb,
 			  __pa(priv->pgtable), priv->redirect,
 			  iommu_drvdata->ttbr_split);
 
@@ -501,7 +499,8 @@
 	SET_TLBIASID(iommu_drvdata->base, ctx_dev->num,
 		     GET_CONTEXTIDR_ASID(iommu_drvdata->base, ctx_dev->num));
 
-	__reset_context(iommu_drvdata->base, ctx_dev->num);
+	__reset_context(iommu_drvdata->base, iommu_drvdata->glb_base,
+			ctx_dev->num);
 
 	msm_iommu_remote_spin_unlock();
 
diff --git a/drivers/iommu/msm_iommu_dev-v2.c b/drivers/iommu/msm_iommu_dev-v2.c
index 5a0b593..cf30500 100644
--- a/drivers/iommu/msm_iommu_dev-v2.c
+++ b/drivers/iommu/msm_iommu_dev-v2.c
@@ -134,6 +134,8 @@
 	if (!drvdata->base)
 		return -ENOMEM;
 
+	drvdata->glb_base = drvdata->base;
+
 	drvdata->gdsc = devm_regulator_get(&pdev->dev, "vdd");
 	if (IS_ERR(drvdata->gdsc))
 		return -EINVAL;
diff --git a/drivers/iommu/msm_iommu_dev.c b/drivers/iommu/msm_iommu_dev.c
index d9eddcd..44fe454 100644
--- a/drivers/iommu/msm_iommu_dev.c
+++ b/drivers/iommu/msm_iommu_dev.c
@@ -23,6 +23,9 @@
 #include <linux/slab.h>
 #include <linux/list.h>
 #include <linux/mutex.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
 
 #include <mach/iommu_hw-8xxx.h>
 #include <mach/iommu.h>
@@ -81,28 +84,28 @@
 }
 EXPORT_SYMBOL(msm_iommu_get_ctx);
 
-static void msm_iommu_reset(void __iomem *base, int ncb)
+static void msm_iommu_reset(void __iomem *base, void __iomem *glb_base, int ncb)
 {
 	int ctx;
 
-	SET_RPUE(base, 0);
-	SET_RPUEIE(base, 0);
-	SET_ESRRESTORE(base, 0);
-	SET_TBE(base, 0);
-	SET_CR(base, 0);
-	SET_SPDMBE(base, 0);
-	SET_TESTBUSCR(base, 0);
-	SET_TLBRSW(base, 0);
-	SET_GLOBAL_TLBIALL(base, 0);
-	SET_RPU_ACR(base, 0);
-	SET_TLBLKCRWE(base, 1);
+	SET_RPUE(glb_base, 0);
+	SET_RPUEIE(glb_base, 0);
+	SET_ESRRESTORE(glb_base, 0);
+	SET_TBE(glb_base, 0);
+	SET_CR(glb_base, 0);
+	SET_SPDMBE(glb_base, 0);
+	SET_TESTBUSCR(glb_base, 0);
+	SET_TLBRSW(glb_base, 0);
+	SET_GLOBAL_TLBIALL(glb_base, 0);
+	SET_RPU_ACR(glb_base, 0);
+	SET_TLBLKCRWE(glb_base, 1);
 
 	for (ctx = 0; ctx < ncb; ctx++) {
-		SET_BPRCOSH(base, ctx, 0);
-		SET_BPRCISH(base, ctx, 0);
-		SET_BPRCNSH(base, ctx, 0);
-		SET_BPSHCFG(base, ctx, 0);
-		SET_BPMTCFG(base, ctx, 0);
+		SET_BPRCOSH(glb_base, ctx, 0);
+		SET_BPRCISH(glb_base, ctx, 0);
+		SET_BPRCNSH(glb_base, ctx, 0);
+		SET_BPSHCFG(glb_base, ctx, 0);
+		SET_BPMTCFG(glb_base, ctx, 0);
 		SET_ACTLR(base, ctx, 0);
 		SET_SCTLR(base, ctx, 0);
 		SET_FSRRESTORE(base, ctx, 0);
@@ -124,135 +127,224 @@
 	mb();
 }
 
+static int msm_iommu_parse_dt(struct platform_device *pdev,
+				struct msm_iommu_drvdata *drvdata)
+{
+#ifdef CONFIG_OF_DEVICE
+	struct device_node *child;
+	struct resource *r;
+	u32 glb_offset = 0;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		pr_err("%s: Missing property reg\n", __func__);
+		return -EINVAL;
+	}
+	drvdata->base = devm_ioremap(&pdev->dev, r->start, resource_size(r));
+	if (!drvdata->base) {
+		pr_err("%s: Unable to ioremap address %x size %x\n", __func__,
+			r->start, resource_size(r));
+		return -ENOMEM;
+	}
+	drvdata->glb_base = drvdata->base;
+
+	if (!of_property_read_u32(pdev->dev.of_node, "qcom,glb-offset",
+			&glb_offset)) {
+		drvdata->glb_base += glb_offset;
+	} else {
+		pr_err("%s: Missing property qcom,glb-offset\n", __func__);
+		return -EINVAL;
+	}
+
+	for_each_child_of_node(pdev->dev.of_node, child) {
+		drvdata->ncb++;
+		if (!of_platform_device_create(child, NULL, &pdev->dev))
+			pr_err("Failed to create %s device\n", child->name);
+	}
+
+	drvdata->name = dev_name(&pdev->dev);
+	drvdata->sec_id = -1;
+	drvdata->ttbr_split = 0;
+#endif
+	return 0;
+}
+
+static int __get_clocks(struct platform_device *pdev,
+				 struct msm_iommu_drvdata *drvdata)
+{
+	int ret = 0;
+
+	drvdata->pclk = clk_get(&pdev->dev, "iface_clk");
+	if (IS_ERR(drvdata->pclk)) {
+		ret = PTR_ERR(drvdata->pclk);
+		drvdata->pclk = NULL;
+		pr_err("Unable to get %s clock for %s IOMMU device\n",
+			dev_name(&pdev->dev), drvdata->name);
+		goto fail;
+	}
+
+	drvdata->clk = clk_get(&pdev->dev, "core_clk");
+
+	if (!IS_ERR(drvdata->clk)) {
+		if (clk_get_rate(drvdata->clk) == 0) {
+			ret = clk_round_rate(drvdata->clk, 1000);
+			clk_set_rate(drvdata->clk, ret);
+		}
+	} else {
+		drvdata->clk = NULL;
+	}
+	return 0;
+fail:
+	return ret;
+}
+
+static void __put_clocks(struct msm_iommu_drvdata *drvdata)
+{
+	if (drvdata->clk)
+		clk_put(drvdata->clk);
+	clk_put(drvdata->pclk);
+}
+
+static int __enable_clocks(struct msm_iommu_drvdata *drvdata)
+{
+	int ret;
+
+	ret = clk_prepare_enable(drvdata->pclk);
+	if (ret)
+		goto fail;
+
+	if (drvdata->clk) {
+		ret = clk_prepare_enable(drvdata->clk);
+		if (ret)
+			clk_disable_unprepare(drvdata->pclk);
+	}
+fail:
+	return ret;
+}
+
+static void __disable_clocks(struct msm_iommu_drvdata *drvdata)
+{
+	if (drvdata->clk)
+		clk_disable_unprepare(drvdata->clk);
+	clk_disable_unprepare(drvdata->pclk);
+}
+
+/*
+ * Do a basic check of the IOMMU by performing an ATS operation
+ * on context bank 0.
+ */
+static int iommu_sanity_check(struct msm_iommu_drvdata *drvdata)
+{
+	int par;
+	int ret = 0;
+
+	SET_M(drvdata->base, 0, 1);
+	SET_PAR(drvdata->base, 0, 0);
+	SET_V2PCFG(drvdata->base, 0, 1);
+	SET_V2PPR(drvdata->base, 0, 0);
+	mb();
+	par = GET_PAR(drvdata->base, 0);
+	SET_V2PCFG(drvdata->base, 0, 0);
+	SET_M(drvdata->base, 0, 0);
+	mb();
+
+	if (!par) {
+		pr_err("%s: Invalid PAR value detected\n", drvdata->name);
+		ret = -ENODEV;
+	}
+	return ret;
+}
+
 static int msm_iommu_probe(struct platform_device *pdev)
 {
-	struct resource *r, *r2;
-	struct clk *iommu_clk = NULL;
-	struct clk *iommu_pclk = NULL;
 	struct msm_iommu_drvdata *drvdata;
 	struct msm_iommu_dev *iommu_dev = pdev->dev.platform_data;
-	void __iomem *regs_base;
-	resource_size_t	len;
-	int ret, par;
+	int ret;
 
-	drvdata = kzalloc(sizeof(*drvdata), GFP_KERNEL);
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(*drvdata), GFP_KERNEL);
 
 	if (!drvdata) {
 		ret = -ENOMEM;
 		goto fail;
 	}
 
-	if (!iommu_dev) {
-		ret = -ENODEV;
-		goto fail;
-	}
+	if (pdev->dev.of_node) {
+		ret = msm_iommu_parse_dt(pdev, drvdata);
+		if (ret)
+			goto fail;
+	} else if (pdev->dev.platform_data) {
+		struct resource *r, *r2;
+		resource_size_t	len;
 
-	iommu_pclk = clk_get_sys("msm_iommu", "iface_clk");
-	if (IS_ERR(iommu_pclk)) {
-		ret = -ENODEV;
-		goto fail;
-	}
+		r = platform_get_resource_byname(pdev, IORESOURCE_MEM,
+						"physbase");
 
-	ret = clk_prepare_enable(iommu_pclk);
-	if (ret)
-		goto fail_enable;
-
-	iommu_clk = clk_get(&pdev->dev, "core_clk");
-
-	if (!IS_ERR(iommu_clk))	{
-		if (clk_get_rate(iommu_clk) == 0) {
-			ret = clk_round_rate(iommu_clk, 1);
-			clk_set_rate(iommu_clk, ret);
+		if (!r) {
+			ret = -ENODEV;
+			goto fail;
 		}
 
-		ret = clk_prepare_enable(iommu_clk);
-		if (ret) {
-			clk_put(iommu_clk);
-			goto fail_pclk;
-		}
-	} else
-		iommu_clk = NULL;
+		len = resource_size(r);
 
-	r = platform_get_resource_byname(pdev, IORESOURCE_MEM, "physbase");
-
-	if (!r) {
-		ret = -ENODEV;
-		goto fail_clk;
-	}
-
-	len = resource_size(r);
-
-	r2 = request_mem_region(r->start, len, r->name);
-	if (!r2) {
-		pr_err("Could not request memory region: start=%p, len=%d\n",
+		r2 = request_mem_region(r->start, len, r->name);
+		if (!r2) {
+			pr_err("Could not request memory region: start=%p, len=%d\n",
 							(void *) r->start, len);
-		ret = -EBUSY;
-		goto fail_clk;
-	}
+			ret = -EBUSY;
+			goto fail;
+		}
 
-	regs_base = ioremap(r2->start, len);
+		drvdata->base = devm_ioremap(&pdev->dev, r2->start, len);
 
-	if (!regs_base) {
-		pr_err("Could not ioremap: start=%p, len=%d\n",
-			 (void *) r2->start, len);
-		ret = -EBUSY;
-		goto fail_mem;
-	}
-
-	msm_iommu_reset(regs_base, iommu_dev->ncb);
-
-	SET_M(regs_base, 0, 1);
-	SET_PAR(regs_base, 0, 0);
-	SET_V2PCFG(regs_base, 0, 1);
-	SET_V2PPR(regs_base, 0, 0);
-	mb();
-	par = GET_PAR(regs_base, 0);
-	SET_V2PCFG(regs_base, 0, 0);
-	SET_M(regs_base, 0, 0);
-	mb();
-
-	if (!par) {
-		pr_err("%s: Invalid PAR value detected\n", iommu_dev->name);
+		if (!drvdata->base) {
+			pr_err("Could not ioremap: start=%p, len=%d\n",
+				 (void *) r2->start, len);
+			ret = -EBUSY;
+			goto fail;
+		}
+		/*
+		 * Global register space offset for legacy IOMMUv1 hardware
+		 * is always 0xFF000
+		 */
+		drvdata->glb_base = drvdata->base + 0xFF000;
+		drvdata->name = iommu_dev->name;
+		drvdata->dev = &pdev->dev;
+		drvdata->ncb = iommu_dev->ncb;
+		drvdata->ttbr_split = iommu_dev->ttbr_split;
+	} else {
 		ret = -ENODEV;
-		goto fail_io;
+		goto fail;
 	}
 
-	drvdata->pclk = iommu_pclk;
-	drvdata->clk = iommu_clk;
-	drvdata->base = regs_base;
-	drvdata->ncb = iommu_dev->ncb;
-	drvdata->ttbr_split = iommu_dev->ttbr_split;
-	drvdata->name = iommu_dev->name;
 	drvdata->dev = &pdev->dev;
 
-	msm_iommu_add_drv(drvdata);
+	ret = __get_clocks(pdev, drvdata);
+
+	if (ret)
+		goto fail;
+
+	__enable_clocks(drvdata);
+
+	msm_iommu_reset(drvdata->base, drvdata->glb_base, drvdata->ncb);
+
+	ret = iommu_sanity_check(drvdata);
+	if (ret)
+		goto fail_clk;
 
 	pr_info("device %s mapped at %p, with %d ctx banks\n",
-		iommu_dev->name, regs_base, iommu_dev->ncb);
+		drvdata->name, drvdata->base, drvdata->ncb);
 
+	msm_iommu_add_drv(drvdata);
 	platform_set_drvdata(pdev, drvdata);
 
-	if (iommu_clk)
-		clk_disable_unprepare(iommu_clk);
-
-	clk_disable_unprepare(iommu_pclk);
+	__disable_clocks(drvdata);
 
 	return 0;
-fail_io:
-	iounmap(regs_base);
-fail_mem:
-	release_mem_region(r->start, len);
+
 fail_clk:
-	if (iommu_clk) {
-		clk_disable_unprepare(iommu_clk);
-		clk_put(iommu_clk);
-	}
-fail_pclk:
-	clk_disable_unprepare(iommu_pclk);
-fail_enable:
-	clk_put(iommu_pclk);
+	__disable_clocks(drvdata);
+	__put_clocks(drvdata);
 fail:
-	kfree(drvdata);
 	return ret;
 }
 
@@ -266,20 +358,116 @@
 		if (drv->clk)
 			clk_put(drv->clk);
 		clk_put(drv->pclk);
-		memset(drv, 0, sizeof(*drv));
-		kfree(drv);
 		platform_set_drvdata(pdev, NULL);
 	}
 	return 0;
 }
 
+static int msm_iommu_ctx_parse_dt(struct platform_device *pdev,
+				struct msm_iommu_ctx_drvdata *ctx_drvdata)
+{
+	struct resource *r, rp;
+	int irq, ret;
+	u32 nmid_array_size;
+	u32 nmid;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq > 0) {
+		ret = request_threaded_irq(irq, NULL,
+				msm_iommu_fault_handler,
+				IRQF_ONESHOT | IRQF_SHARED,
+				"msm_iommu_nonsecure_irq", pdev);
+		if (ret) {
+			pr_err("Request IRQ %d failed with ret=%d\n", irq, ret);
+			return ret;
+		}
+	}
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!r) {
+		pr_err("Could not find reg property for context bank\n");
+		return -EINVAL;
+	}
+
+	ret = of_address_to_resource(pdev->dev.parent->of_node, 0, &rp);
+	if (ret) {
+		pr_err("of_address_to_resource failed\n");
+		return -EINVAL;
+	}
+
+	/* Calculate the context bank number using the base addresses. CB0
+	 * starts at the base address.
+	 */
+	ctx_drvdata->num = ((r->start - rp.start) >> CTX_SHIFT);
+
+	if (of_property_read_string(pdev->dev.of_node, "label",
+					&ctx_drvdata->name)) {
+		pr_err("Could not find label property\n");
+		return -EINVAL;
+	}
+
+	if (!of_get_property(pdev->dev.of_node, "qcom,iommu-ctx-mids",
+			     &nmid_array_size)) {
+		pr_err("Could not find iommu-ctx-mids property\n");
+		return -EINVAL;
+	}
+	if (nmid_array_size >= sizeof(ctx_drvdata->sids)) {
+		pr_err("Too many mids defined - array size: %u, mids size: %u\n",
+			nmid_array_size, sizeof(ctx_drvdata->sids));
+		return -EINVAL;
+	}
+	nmid = nmid_array_size / sizeof(*ctx_drvdata->sids);
+
+	if (of_property_read_u32_array(pdev->dev.of_node, "qcom,iommu-ctx-mids",
+				       ctx_drvdata->sids, nmid)) {
+		pr_err("Could not find iommu-ctx-mids property\n");
+		return -EINVAL;
+	}
+	ctx_drvdata->nsid = nmid;
+
+	return 0;
+}
+
+static void __program_m2v_tables(struct msm_iommu_drvdata *drvdata,
+				struct msm_iommu_ctx_drvdata *ctx_drvdata)
+{
+	int i;
+
+	/* Program the M2V tables for this context */
+	for (i = 0; i < ctx_drvdata->nsid; i++) {
+		int sid = ctx_drvdata->sids[i];
+		int num = ctx_drvdata->num;
+
+		SET_M2VCBR_N(drvdata->glb_base, sid, 0);
+		SET_CBACR_N(drvdata->glb_base, num, 0);
+
+		/* Route page faults to the non-secure interrupt */
+		SET_IRPTNDX(drvdata->glb_base, num, 1);
+
+		/* Set VMID = 0 */
+		SET_VMID(drvdata->glb_base, sid, 0);
+
+		/* Set the context number for that SID to this context */
+		SET_CBNDX(drvdata->glb_base, sid, num);
+
+		/* Set SID associated with this context bank to 0 */
+		SET_CBVMID(drvdata->glb_base, num, 0);
+
+		/* Set the ASID for TLB tagging for this context to 0 */
+		SET_CONTEXTIDR_ASID(drvdata->base, num, 0);
+
+		/* Set security bit override to be Non-secure */
+		SET_NSCFG(drvdata->glb_base, sid, 3);
+	}
+	mb();
+}
+
 static int msm_iommu_ctx_probe(struct platform_device *pdev)
 {
-	struct msm_iommu_ctx_dev *c = pdev->dev.platform_data;
 	struct msm_iommu_drvdata *drvdata;
 	struct msm_iommu_ctx_drvdata *ctx_drvdata = NULL;
 	int i, ret, irq;
-	if (!c || !pdev->dev.parent) {
+	if (!pdev->dev.parent) {
 		ret = -EINVAL;
 		goto fail;
 	}
@@ -291,112 +479,100 @@
 		goto fail;
 	}
 
-	ctx_drvdata = kzalloc(sizeof(*ctx_drvdata), GFP_KERNEL);
+	ctx_drvdata = devm_kzalloc(&pdev->dev, sizeof(*ctx_drvdata),
+					GFP_KERNEL);
 	if (!ctx_drvdata) {
 		ret = -ENOMEM;
 		goto fail;
 	}
-	ctx_drvdata->num = c->num;
-	ctx_drvdata->pdev = pdev;
-	ctx_drvdata->name = c->name;
 
-	irq = platform_get_irq_byname(to_platform_device(pdev->dev.parent),
-				      "nonsecure_irq");
-	if (irq < 0) {
+	ctx_drvdata->pdev = pdev;
+	INIT_LIST_HEAD(&ctx_drvdata->attached_elm);
+	platform_set_drvdata(pdev, ctx_drvdata);
+
+	if (pdev->dev.of_node) {
+		ret = msm_iommu_ctx_parse_dt(pdev, ctx_drvdata);
+		if (ret)
+			goto fail;
+	} else if (pdev->dev.platform_data) {
+		struct msm_iommu_ctx_dev *c = pdev->dev.platform_data;
+
+		ctx_drvdata->num = c->num;
+		ctx_drvdata->name = c->name;
+
+		for (i = 0;  i < MAX_NUM_MIDS; ++i) {
+			if (c->mids[i] == -1) {
+				ctx_drvdata->nsid = i;
+				break;
+			}
+			ctx_drvdata->sids[i] = c->mids[i];
+		}
+		irq = platform_get_irq_byname(
+					to_platform_device(pdev->dev.parent),
+					"nonsecure_irq");
+		if (irq < 0) {
+			ret = -ENODEV;
+			goto fail;
+		}
+
+		ret = request_threaded_irq(irq, NULL, msm_iommu_fault_handler,
+					IRQF_ONESHOT | IRQF_SHARED,
+					"msm_iommu_nonsecure_irq", ctx_drvdata);
+
+		if (ret) {
+			pr_err("request_threaded_irq %d failed: %d\n", irq,
+								       ret);
+			goto fail;
+		}
+	} else {
 		ret = -ENODEV;
 		goto fail;
 	}
 
-	ret = request_threaded_irq(irq, NULL, msm_iommu_fault_handler,
-				   IRQF_ONESHOT | IRQF_SHARED,
-				   "msm_iommu_nonsecure_irq", ctx_drvdata);
+	__enable_clocks(drvdata);
+	__program_m2v_tables(drvdata, ctx_drvdata);
+	__disable_clocks(drvdata);
 
-	if (ret) {
-		pr_err("request_threaded_irq %d failed: %d\n", irq, ret);
-		goto fail;
-	}
-
-	INIT_LIST_HEAD(&ctx_drvdata->attached_elm);
-	platform_set_drvdata(pdev, ctx_drvdata);
-
-	ret = clk_prepare_enable(drvdata->pclk);
-	if (ret)
-		goto fail;
-
-	if (drvdata->clk) {
-		ret = clk_prepare_enable(drvdata->clk);
-		if (ret) {
-			clk_disable_unprepare(drvdata->pclk);
-			goto fail;
-		}
-	}
-
-	/* Program the M2V tables for this context */
-	for (i = 0; i < MAX_NUM_MIDS; i++) {
-		int mid = c->mids[i];
-		if (mid == -1)
-			break;
-
-		SET_M2VCBR_N(drvdata->base, mid, 0);
-		SET_CBACR_N(drvdata->base, c->num, 0);
-
-		/* Route page faults to the non-secure interrupt */
-		SET_IRPTNDX(drvdata->base, c->num, 1);
-
-		/* Set VMID = 0 */
-		SET_VMID(drvdata->base, mid, 0);
-
-		/* Set the context number for that MID to this context */
-		SET_CBNDX(drvdata->base, mid, c->num);
-
-		/* Set MID associated with this context bank to 0 */
-		SET_CBVMID(drvdata->base, c->num, 0);
-
-		/* Set the ASID for TLB tagging for this context to 0 */
-		SET_CONTEXTIDR_ASID(drvdata->base, c->num, 0);
-
-		/* Set security bit override to be Non-secure */
-		SET_NSCFG(drvdata->base, mid, 3);
-	}
-	mb();
-
-	if (drvdata->clk)
-		clk_disable_unprepare(drvdata->clk);
-	clk_disable_unprepare(drvdata->pclk);
-
-	dev_info(&pdev->dev, "context %s using bank %d\n", c->name, c->num);
+	dev_info(&pdev->dev, "context %s using bank %d\n", ctx_drvdata->name,
+							   ctx_drvdata->num);
 	return 0;
 fail:
-	kfree(ctx_drvdata);
 	return ret;
 }
 
-static int msm_iommu_ctx_remove(struct platform_device *pdev)
+static int __devexit msm_iommu_ctx_remove(struct platform_device *pdev)
 {
-	struct msm_iommu_ctx_drvdata *drv = NULL;
-	drv = platform_get_drvdata(pdev);
-	if (drv) {
-		memset(drv, 0, sizeof(struct msm_iommu_ctx_drvdata));
-		kfree(drv);
-		platform_set_drvdata(pdev, NULL);
-	}
+	platform_set_drvdata(pdev, NULL);
 	return 0;
 }
 
+
+static struct of_device_id msm_iommu_match_table[] = {
+	{ .compatible = "qcom,msm-smmu-v1", },
+	{}
+};
+
 static struct platform_driver msm_iommu_driver = {
 	.driver = {
 		.name	= "msm_iommu",
+		.of_match_table = msm_iommu_match_table,
 	},
 	.probe		= msm_iommu_probe,
-	.remove		= msm_iommu_remove,
+	.remove		= __devexit_p(msm_iommu_remove),
+};
+
+static struct of_device_id msm_iommu_ctx_match_table[] = {
+	{ .name = "qcom,iommu-ctx", },
+	{}
 };
 
 static struct platform_driver msm_iommu_ctx_driver = {
 	.driver = {
 		.name	= "msm_iommu_ctx",
+		.of_match_table = msm_iommu_ctx_match_table,
 	},
 	.probe		= msm_iommu_ctx_probe,
-	.remove		= msm_iommu_ctx_remove,
+	.remove		= __devexit_p(msm_iommu_ctx_remove),
 };
 
 static int __init msm_iommu_driver_init(void)
diff --git a/drivers/iommu/msm_iommu_sec.c b/drivers/iommu/msm_iommu_sec.c
index 72ec4a6..a6483b9 100644
--- a/drivers/iommu/msm_iommu_sec.c
+++ b/drivers/iommu/msm_iommu_sec.c
@@ -74,9 +74,9 @@
 		unsigned int spare;
 	} pinit;
 	unsigned int *buf;
-	int psize[2] = {0};
+	int psize[2] = {0, 0};
 	unsigned int spare;
-	int ret, ptbl_ret;
+	int ret, ptbl_ret = 0;
 
 	for_each_compatible_node(np, NULL, "qcom,msm-smmu-v2")
 		if (of_find_property(np, "qcom,iommu-secure-id", NULL))
@@ -134,7 +134,7 @@
 		unsigned int id;
 		unsigned int spare;
 	} cfg;
-	int ret, scm_ret;
+	int ret, scm_ret = 0;
 
 	cfg.id = sec_id;
 
diff --git a/drivers/leds/leds-pm8xxx.c b/drivers/leds/leds-pm8xxx.c
index a641ce9..f493129 100644
--- a/drivers/leds/leds-pm8xxx.c
+++ b/drivers/leds/leds-pm8xxx.c
@@ -70,6 +70,7 @@
 #define WLED_CTL_DLY_BIT_SHFT		0x05
 #define WLED_MAX_CURR			25
 #define WLED_MAX_CURR_MASK		0x1F
+#define WLED_BRIGHTNESS_MSB_MASK	0x0F
 #define WLED_OP_FDBCK_MASK		0x1C
 #define WLED_OP_FDBCK_BIT_SHFT		0x02
 
@@ -283,7 +284,8 @@
 			return rc;
 		}
 
-		val = (val & ~WLED_MAX_CURR_MASK) | (duty >> WLED_8_BIT_SHFT);
+		val = (val & ~WLED_BRIGHTNESS_MSB_MASK) |
+			(duty >> WLED_8_BIT_SHFT);
 		rc = pm8xxx_writeb(led->dev->parent,
 				WLED_BRIGHTNESS_CNTL_REG1(i), val);
 		if (rc) {
diff --git a/drivers/media/dvb/mpq/adapter/mpq_stream_buffer.c b/drivers/media/dvb/mpq/adapter/mpq_stream_buffer.c
index 4b0e7be..f779851 100644
--- a/drivers/media/dvb/mpq/adapter/mpq_stream_buffer.c
+++ b/drivers/media/dvb/mpq/adapter/mpq_stream_buffer.c
@@ -14,17 +14,47 @@
 #include <linux/module.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
+#include <linux/uaccess.h>
 #include "mpq_dvb_debug.h"
 #include "mpq_stream_buffer.h"
 
 
-void mpq_streambuffer_init(
+
+
+int mpq_streambuffer_init(
 		struct mpq_streambuffer *sbuff,
-		void *data_buff, size_t data_buff_len,
-		void *packet_buff, size_t packet_buff_size)
+		enum mpq_streambuffer_mode mode,
+		struct mpq_streambuffer_buffer_desc *data_buffers,
+		u32 data_buff_num,
+		void *packet_buff,
+		size_t packet_buff_size)
 {
-	dvb_ringbuffer_init(&sbuff->raw_data, data_buff, data_buff_len);
+	if ((NULL == sbuff) || (NULL == data_buffers) || (NULL == packet_buff))
+		return -EINVAL;
+
+	if (data_buff_num > 1) {
+		if (mode != MPQ_STREAMBUFFER_BUFFER_MODE_LINEAR)
+			return -EINVAL;
+		/* Linear buffer group */
+		dvb_ringbuffer_init(
+			&sbuff->raw_data,
+			data_buffers,
+			data_buff_num *
+			sizeof(struct mpq_streambuffer_buffer_desc));
+	} else if (data_buff_num == 1) {
+		if (mode != MPQ_STREAMBUFFER_BUFFER_MODE_RING)
+			return -EINVAL;
+		/* Single ring-buffer */
+		dvb_ringbuffer_init(&sbuff->raw_data,
+			data_buffers[0].base, data_buffers[0].size);
+	}
+	sbuff->mode = mode;
+	sbuff->buffers = data_buffers;
+	sbuff->pending_buffers_count = 0;
+	sbuff->buffers_num = data_buff_num;
 	dvb_ringbuffer_init(&sbuff->packet_data, packet_buff, packet_buff_size);
+
+	return 0;
 }
 EXPORT_SYMBOL(mpq_streambuffer_init);
 
@@ -87,34 +117,55 @@
 	int ret;
 	struct mpq_streambuffer_packet_header packet;
 
-	if (dispose_data) {
-		/* read-out the packet header first */
-		ret = dvb_ringbuffer_pkt_read(
-				&sbuff->packet_data,
-				idx,
-				0,
-				(u8 *)&packet,
-				sizeof(struct mpq_streambuffer_packet_header));
+	if (NULL == sbuff)
+		return -EINVAL;
 
-		if (ret != sizeof(struct mpq_streambuffer_packet_header))
-			return -EINVAL;
+	/* read-out the packet header first */
+	ret = dvb_ringbuffer_pkt_read(&sbuff->packet_data, idx,
+			0,
+			(u8 *)&packet,
+			sizeof(struct mpq_streambuffer_packet_header));
 
+	if (ret != sizeof(struct mpq_streambuffer_packet_header))
+		return -EINVAL;
+
+	if ((MPQ_STREAMBUFFER_BUFFER_MODE_LINEAR == sbuff->mode) ||
+		(dispose_data)) {
 		/* Advance the read pointer in the raw-data buffer first */
-		ret = mpq_streambuffer_data_read_dispose(
-							sbuff,
-							packet.raw_data_len);
+		ret = mpq_streambuffer_data_read_dispose(sbuff,
+				packet.raw_data_len);
 		if (ret != 0)
 			return ret;
 	}
 
+	/* Move read pointer to the next linear buffer for subsequent reads */
+	if ((MPQ_STREAMBUFFER_BUFFER_MODE_LINEAR == sbuff->mode) &&
+		(packet.raw_data_len > 0)) {
+		struct mpq_streambuffer_buffer_desc *desc;
+
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pread];
+
+		desc->write_ptr = 0;
+		desc->read_ptr = 0;
+
+		DVB_RINGBUFFER_SKIP(&sbuff->raw_data,
+				sizeof(struct mpq_streambuffer_buffer_desc));
+		sbuff->pending_buffers_count--;
+
+		wake_up_all(&sbuff->raw_data.queue);
+	}
+
 	/* Now clear the packet from the packet header */
 	dvb_ringbuffer_pkt_dispose(&sbuff->packet_data, idx);
 
+	if (sbuff->cb)
+		sbuff->cb(sbuff, sbuff->cb_user_data);
+
 	return 0;
 }
 EXPORT_SYMBOL(mpq_streambuffer_pkt_dispose);
 
-
 int mpq_streambuffer_pkt_write(
 			struct mpq_streambuffer *sbuff,
 			struct mpq_streambuffer_packet_header *packet,
@@ -123,30 +174,48 @@
 	ssize_t idx;
 	size_t len;
 
-	len =
-		sizeof(struct mpq_streambuffer_packet_header) +
+	if ((NULL == sbuff) || (NULL == packet))
+		return -EINVAL;
+
+	MPQ_DVB_DBG_PRINT(
+		"%s: handle=%d, offset=%d, len=%d\n",
+		__func__,
+		packet->raw_data_handle,
+		packet->raw_data_offset,
+		packet->raw_data_len);
+
+	len = sizeof(struct mpq_streambuffer_packet_header) +
 		packet->user_data_len;
 
 	/* Make sure enough space available for packet header */
 	if (dvb_ringbuffer_free(&sbuff->packet_data) < len)
 		return -ENOSPC;
 
-	/* Starting writting packet header */
+	/* Starting writing packet header */
 	idx = dvb_ringbuffer_pkt_start(&sbuff->packet_data, len);
 
 	/* Write non-user private data header */
-	dvb_ringbuffer_write(
-				&sbuff->packet_data,
-				(u8 *)packet,
-				sizeof(struct mpq_streambuffer_packet_header));
+	dvb_ringbuffer_write(&sbuff->packet_data,
+		(u8 *)packet,
+		sizeof(struct mpq_streambuffer_packet_header));
 
 	/* Write user's own private data header */
 	dvb_ringbuffer_write(&sbuff->packet_data,
-						 user_data,
-						 packet->user_data_len);
+		user_data,
+		packet->user_data_len);
 
 	dvb_ringbuffer_pkt_close(&sbuff->packet_data, idx);
 
+	/* Move write pointer to next linear buffer for subsequent writes */
+	if ((MPQ_STREAMBUFFER_BUFFER_MODE_LINEAR == sbuff->mode) &&
+		(packet->raw_data_len > 0)) {
+		if (sbuff->pending_buffers_count == sbuff->buffers_num)
+			return -ENOSPC;
+		DVB_RINGBUFFER_PUSH(&sbuff->raw_data,
+				sizeof(struct mpq_streambuffer_buffer_desc));
+		sbuff->pending_buffers_count++;
+	}
+
 	wake_up_all(&sbuff->packet_data.queue);
 
 	return 0;
@@ -160,11 +229,52 @@
 {
 	int res;
 
-	if (unlikely(dvb_ringbuffer_free(&sbuff->raw_data) < len))
-		return -ENOSPC;
+	if ((NULL == sbuff) || (NULL == buf))
+		return -EINVAL;
 
-	res = dvb_ringbuffer_write(&sbuff->raw_data, buf, len);
-	wake_up_all(&sbuff->raw_data.queue);
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		if (unlikely(dvb_ringbuffer_free(&sbuff->raw_data) < len))
+			return -ENOSPC;
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == sbuff->raw_data.data)
+			return -EPERM;
+		res = dvb_ringbuffer_write(&sbuff->raw_data, buf, len);
+		wake_up_all(&sbuff->raw_data.queue);
+	} else {
+		/* Linear buffer group */
+		struct mpq_streambuffer_buffer_desc *desc;
+
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pwrite];
+
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == desc->base)
+			return -EPERM;
+
+		if ((sbuff->pending_buffers_count == sbuff->buffers_num) ||
+			((desc->size - desc->write_ptr) < len)) {
+			MPQ_DVB_ERR_PRINT(
+				"%s: No space available! %d pending buffers out of %d total buffers. write_ptr=%d, size=%d\n",
+				__func__,
+				sbuff->pending_buffers_count,
+				sbuff->buffers_num,
+				desc->write_ptr,
+				desc->size);
+			return -ENOSPC;
+		}
+		memcpy(desc->base + desc->write_ptr, buf, len);
+		desc->write_ptr += len;
+		MPQ_DVB_DBG_PRINT(
+			"%s: copied %d data bytes. handle=%d, write_ptr=%d\n",
+			__func__, len, desc->handle, desc->write_ptr);
+		res = len;
+	}
 
 	return res;
 }
@@ -175,50 +285,244 @@
 				struct mpq_streambuffer *sbuff,
 				size_t len)
 {
+	if (NULL == sbuff)
+		return -EINVAL;
+
 	if (unlikely(dvb_ringbuffer_free(&sbuff->raw_data) < len))
 		return -ENOSPC;
 
-	sbuff->raw_data.pwrite =
-		(sbuff->raw_data.pwrite+len) % sbuff->raw_data.size;
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		DVB_RINGBUFFER_PUSH(&sbuff->raw_data, len);
+		wake_up_all(&sbuff->raw_data.queue);
+	} else {
+		/* Linear buffer group */
+		struct mpq_streambuffer_buffer_desc *desc;
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pwrite];
 
-	wake_up_all(&sbuff->raw_data.queue);
+		if ((sbuff->pending_buffers_count == sbuff->buffers_num) ||
+			 ((desc->size - desc->write_ptr) < len)) {
+			MPQ_DVB_ERR_PRINT(
+				"%s: No space available!\n",
+				__func__);
+			return -ENOSPC;
+		}
+		desc->write_ptr += len;
+	}
 
 	return 0;
 }
 EXPORT_SYMBOL(mpq_streambuffer_data_write_deposit);
 
 
-size_t mpq_streambuffer_data_read(
+ssize_t mpq_streambuffer_data_read(
 				struct mpq_streambuffer *sbuff,
 				u8 *buf, size_t len)
 {
-	ssize_t actual_len;
+	ssize_t actual_len = 0;
 
-	actual_len = dvb_ringbuffer_avail(&sbuff->raw_data);
-	if (actual_len < len)
-		len = actual_len;
+	if ((NULL == sbuff) || (NULL == buf))
+		return -EINVAL;
 
-	if (len)
-		dvb_ringbuffer_read(&sbuff->raw_data, buf, len);
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == sbuff->raw_data.data)
+			return -EPERM;
 
-	wake_up_all(&sbuff->raw_data.queue);
+		actual_len = dvb_ringbuffer_avail(&sbuff->raw_data);
+		if (actual_len < len)
+			len = actual_len;
+		if (len)
+			dvb_ringbuffer_read(&sbuff->raw_data, buf, len);
+
+		wake_up_all(&sbuff->raw_data.queue);
+	} else {
+		/* Linear buffer group */
+		struct mpq_streambuffer_buffer_desc *desc;
+
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pread];
+
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == desc->base)
+			return -EPERM;
+
+		actual_len = (desc->write_ptr - desc->read_ptr);
+		if (actual_len < len)
+			len = actual_len;
+		memcpy(buf, desc->base + desc->read_ptr, len);
+		desc->read_ptr += len;
+	}
 
 	return len;
 }
 EXPORT_SYMBOL(mpq_streambuffer_data_read);
 
 
+ssize_t mpq_streambuffer_data_read_user(
+		struct mpq_streambuffer *sbuff,
+		u8 __user *buf, size_t len)
+{
+	ssize_t actual_len = 0;
+
+	if ((NULL == sbuff) || (NULL == buf))
+		return -EINVAL;
+
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == sbuff->raw_data.data)
+			return -EPERM;
+
+		actual_len = dvb_ringbuffer_avail(&sbuff->raw_data);
+		if (actual_len < len)
+			len = actual_len;
+		if (len)
+			dvb_ringbuffer_read_user(&sbuff->raw_data, buf, len);
+		wake_up_all(&sbuff->raw_data.queue);
+	} else {
+		/* Linear buffer group */
+		struct mpq_streambuffer_buffer_desc *desc;
+
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pread];
+
+		/*
+		 * Secure buffers are not permitted to be mapped into kernel
+		 * memory, and so buffer base address may be NULL
+		 */
+		if (NULL == desc->base)
+			return -EPERM;
+
+		actual_len = (desc->write_ptr - desc->read_ptr);
+		if (actual_len < len)
+			len = actual_len;
+		if (copy_to_user(buf, desc->base + desc->read_ptr, len))
+			return -EFAULT;
+		desc->read_ptr += len;
+	}
+
+	return len;
+}
+EXPORT_SYMBOL(mpq_streambuffer_data_read_user);
+
+
 int mpq_streambuffer_data_read_dispose(
 			struct mpq_streambuffer *sbuff,
 			size_t len)
 {
-	if (unlikely(dvb_ringbuffer_avail(&sbuff->raw_data) < len))
+	if (NULL == sbuff)
 		return -EINVAL;
 
-	DVB_RINGBUFFER_SKIP(&sbuff->raw_data, len);
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		if (unlikely(dvb_ringbuffer_avail(&sbuff->raw_data) < len))
+			return -EINVAL;
 
-	wake_up_all(&sbuff->raw_data.queue);
+		DVB_RINGBUFFER_SKIP(&sbuff->raw_data, len);
+		wake_up_all(&sbuff->raw_data.queue);
+	} else {
+		struct mpq_streambuffer_buffer_desc *desc;
+
+		desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pread];
+		if ((desc->read_ptr + len) > desc->size)
+			desc->read_ptr = desc->size;
+		else
+			desc->read_ptr += len;
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(mpq_streambuffer_data_read_dispose);
 
+
+int mpq_streambuffer_get_buffer_handle(
+	struct mpq_streambuffer *sbuff,
+	int read_buffer,
+	int *handle)
+{
+	struct mpq_streambuffer_buffer_desc *desc = NULL;
+
+	if ((NULL == sbuff) || (NULL == handle))
+		return -EINVAL;
+
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode) {
+		*handle = sbuff->buffers[0].handle;
+	} else {
+		if (read_buffer)
+			desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pread];
+		else
+			desc = (struct mpq_streambuffer_buffer_desc *)
+				&sbuff->raw_data.data[sbuff->raw_data.pwrite];
+		*handle = desc->handle;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(mpq_streambuffer_get_buffer_handle);
+
+
+int mpq_streambuffer_register_pkt_dispose(
+	struct mpq_streambuffer *sbuff,
+	mpq_streambuffer_pkt_dispose_cb cb_func,
+	void *user_data)
+{
+	if ((NULL == sbuff) || (NULL == cb_func))
+		return -EINVAL;
+
+	sbuff->cb = cb_func;
+	sbuff->cb_user_data = user_data;
+
+	return 0;
+}
+EXPORT_SYMBOL(mpq_streambuffer_register_pkt_dispose);
+
+
+ssize_t mpq_streambuffer_data_free(
+	struct mpq_streambuffer *sbuff)
+{
+	struct mpq_streambuffer_buffer_desc *desc;
+
+	if (NULL == sbuff)
+		return -EINVAL;
+
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode)
+		return dvb_ringbuffer_free(&sbuff->raw_data);
+
+	if (sbuff->pending_buffers_count == sbuff->buffers_num)
+		return 0;
+
+	desc = (struct mpq_streambuffer_buffer_desc *)
+		&sbuff->raw_data.data[sbuff->raw_data.pwrite];
+
+	return desc->size - desc->write_ptr;
+}
+EXPORT_SYMBOL(mpq_streambuffer_data_free);
+
+
+ssize_t mpq_streambuffer_data_avail(
+	struct mpq_streambuffer *sbuff)
+{
+	struct mpq_streambuffer_buffer_desc *desc;
+
+	if (NULL == sbuff)
+		return -EINVAL;
+
+	if (MPQ_STREAMBUFFER_BUFFER_MODE_RING == sbuff->mode)
+		return dvb_ringbuffer_avail(&sbuff->raw_data);
+
+	desc = (struct mpq_streambuffer_buffer_desc *)
+		&sbuff->raw_data.data[sbuff->raw_data.pread];
+
+	return desc->write_ptr - desc->read_ptr;
+}
+EXPORT_SYMBOL(mpq_streambuffer_data_avail);
+
diff --git a/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.c b/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.c
index e2ef6a0..2a60840 100644
--- a/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.c
+++ b/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.c
@@ -13,6 +13,7 @@
 #include <linux/init.h>
 #include <linux/module.h>
 #include <linux/vmalloc.h>
+#include <linux/file.h>
 #include "mpq_dvb_debug.h"
 #include "mpq_dmx_plugin_common.h"
 
@@ -983,6 +984,19 @@
 		goto init_failed_free_payload_buffer;
 	}
 
+	feed_data->buffer_desc.read_ptr = 0;
+	feed_data->buffer_desc.write_ptr = 0;
+	feed_data->buffer_desc.base = payload_buffer;
+	feed_data->buffer_desc.size = actual_buffer_size;
+	feed_data->buffer_desc.handle =
+		ion_share_dma_buf(
+			mpq_demux->ion_client,
+			feed_data->payload_buff_handle);
+	if (feed_data->buffer_desc.handle < 0) {
+		ret = -EFAULT;
+		goto init_failed_unmap_payload_buffer;
+	}
+
 	/* Register the new stream-buffer interface to MPQ adapter */
 	switch (feed->pes_type) {
 	case DMX_TS_PES_VIDEO0:
@@ -1011,7 +1025,7 @@
 			__func__,
 			feed->pes_type);
 		ret = -EINVAL;
-		goto init_failed_unmap_payload_buffer;
+		goto init_failed_unshare_payload_buffer;
 	}
 
 	/* make sure not occupied already */
@@ -1025,30 +1039,36 @@
 			__func__,
 			feed_data->stream_interface);
 		ret = -EBUSY;
-		goto init_failed_unmap_payload_buffer;
+		goto init_failed_unshare_payload_buffer;
 	}
 
 	feed_data->video_buffer =
 		&mpq_dmx_info.decoder_buffers[feed_data->stream_interface];
 
-	mpq_streambuffer_init(
-			feed_data->video_buffer,
-			payload_buffer,
-			actual_buffer_size,
-			packet_buffer,
-			VIDEO_META_DATA_BUFFER_SIZE);
+	ret = mpq_streambuffer_init(
+		feed_data->video_buffer,
+		MPQ_STREAMBUFFER_BUFFER_MODE_RING,
+		&feed_data->buffer_desc,
+		1,
+		packet_buffer,
+		VIDEO_META_DATA_BUFFER_SIZE);
+	if (ret < 0) {
+		MPQ_DVB_ERR_PRINT(
+			"%s: mpq_streambuffer_init failed, err = %d\n",
+			__func__, ret);
+		goto init_failed_unshare_payload_buffer;
+	}
 
-	ret =
-		mpq_adapter_register_stream_if(
-			feed_data->stream_interface,
-			feed_data->video_buffer);
+	ret = mpq_adapter_register_stream_if(
+		feed_data->stream_interface,
+		feed_data->video_buffer);
 
 	if (ret < 0) {
 		MPQ_DVB_ERR_PRINT(
 			"%s: mpq_adapter_register_stream_if failed, "
 			"err = %d\n",
 			__func__, ret);
-		goto init_failed_unmap_payload_buffer;
+		goto init_failed_unshare_payload_buffer;
 	}
 
 	feed->buffer_size = actual_buffer_size;
@@ -1067,7 +1087,13 @@
 			sizeof(struct mpq_framing_prefix_size_masks));
 	feed_data->first_pattern_offset = 0;
 	feed_data->first_prefix_size = 0;
-	feed_data->write_pts_dts = 0;
+	feed_data->saved_pts_dts_info.pts_exist = 0;
+	feed_data->saved_pts_dts_info.dts_exist = 0;
+	feed_data->new_pts_dts_info.pts_exist = 0;
+	feed_data->new_pts_dts_info.dts_exist = 0;
+	feed_data->saved_info_used = 1;
+	feed_data->new_info_exists = 0;
+	feed_data->first_pts_dts_copy = 1;
 
 	spin_lock(&mpq_demux->feed_lock);
 	feed->priv = (void *)feed_data;
@@ -1075,6 +1101,8 @@
 
 	return 0;
 
+init_failed_unshare_payload_buffer:
+	put_unused_fd(feed_data->buffer_desc.handle);
 init_failed_unmap_payload_buffer:
 	ion_unmap_kernel(mpq_demux->ion_client,
 					 feed_data->payload_buff_handle);
@@ -1119,6 +1147,8 @@
 
 	vfree(feed_data->video_buffer->packet_data.data);
 
+	put_unused_fd(feed_data->buffer_desc.handle);
+
 	ion_unmap_kernel(mpq_demux->ion_client,
 					 feed_data->payload_buff_handle);
 
@@ -1338,6 +1368,83 @@
 	return 0;
 }
 
+static inline void mpq_dmx_save_pts_dts(struct mpq_video_feed_info *feed_data)
+{
+	if (feed_data->new_info_exists) {
+		feed_data->saved_pts_dts_info.pts_exist =
+			feed_data->new_pts_dts_info.pts_exist;
+		feed_data->saved_pts_dts_info.pts =
+			feed_data->new_pts_dts_info.pts;
+		feed_data->saved_pts_dts_info.dts_exist =
+			feed_data->new_pts_dts_info.dts_exist;
+		feed_data->saved_pts_dts_info.dts =
+			feed_data->new_pts_dts_info.dts;
+
+		feed_data->new_info_exists = 0;
+		feed_data->saved_info_used = 0;
+	}
+}
+
+static inline void mpq_dmx_write_pts_dts(struct mpq_video_feed_info *feed_data,
+					struct dmx_pts_dts_info *info)
+{
+	if (!feed_data->saved_info_used) {
+		info->pts_exist = feed_data->saved_pts_dts_info.pts_exist;
+		info->pts = feed_data->saved_pts_dts_info.pts;
+		info->dts_exist = feed_data->saved_pts_dts_info.dts_exist;
+		info->dts = feed_data->saved_pts_dts_info.dts;
+
+		feed_data->saved_info_used = 1;
+	} else {
+		info->pts_exist = 0;
+		info->dts_exist = 0;
+	}
+}
+
+static inline void mpq_dmx_get_pts_dts(struct mpq_video_feed_info *feed_data,
+				struct pes_packet_header *pes_header)
+{
+	struct dmx_pts_dts_info *info = &(feed_data->new_pts_dts_info);
+
+	/* Get PTS/DTS information from PES header */
+
+	if ((pes_header->pts_dts_flag == 2) ||
+		(pes_header->pts_dts_flag == 3)) {
+		info->pts_exist = 1;
+
+		info->pts =
+			((u64)pes_header->pts_1 << 30) |
+			((u64)pes_header->pts_2 << 22) |
+			((u64)pes_header->pts_3 << 15) |
+			((u64)pes_header->pts_4 << 7) |
+			(u64)pes_header->pts_5;
+	} else {
+		info->pts_exist = 0;
+		info->pts = 0;
+	}
+
+	if (pes_header->pts_dts_flag == 3) {
+		info->dts_exist = 1;
+
+		info->dts =
+			((u64)pes_header->dts_1 << 30) |
+			((u64)pes_header->dts_2 << 22) |
+			((u64)pes_header->dts_3 << 15) |
+			((u64)pes_header->dts_4 << 7) |
+			(u64)pes_header->dts_5;
+	} else {
+		info->dts_exist = 0;
+		info->dts = 0;
+	}
+
+	feed_data->new_info_exists = 1;
+
+	if (feed_data->first_pts_dts_copy) {
+		mpq_dmx_save_pts_dts(feed_data);
+		feed_data->first_pts_dts_copy = 0;
+	}
+}
+
 static inline int mpq_dmx_parse_remaining_pes_header(
 				struct dvb_demux_feed *feed,
 				struct mpq_video_feed_info *feed_data,
@@ -1381,7 +1488,6 @@
 		/* else - we have the PTS */
 		*bytes_avail -= copy_len;
 		*ts_payload_offset += copy_len;
-		feed_data->write_pts_dts = 1;
 	}
 
 	/* Did we capture the DTS value (if exist)? */
@@ -1412,7 +1518,6 @@
 		/* else - we have the DTS */
 		*bytes_avail -= copy_len;
 		*ts_payload_offset += copy_len;
-		feed_data->write_pts_dts = 1;
 	}
 
 	/* Any more header bytes?! */
@@ -1421,6 +1526,9 @@
 		return -EINVAL;
 	}
 
+	/* get PTS/DTS information from PES header to be written later */
+	mpq_dmx_get_pts_dts(feed_data, pes_header);
+
 	/* Got PES header, process payload */
 	*bytes_avail -= feed_data->pes_header_left_bytes;
 	*ts_payload_offset += feed_data->pes_header_left_bytes;
@@ -1429,53 +1537,6 @@
 	return 0;
 }
 
-static inline void mpq_dmx_get_pts_dts(struct mpq_video_feed_info *feed_data,
-				struct pes_packet_header *pes_header,
-				struct mpq_adapter_video_meta_data *meta_data,
-				enum dmx_packet_type packet_type)
-{
-	struct dmx_pts_dts_info *info;
-
-	if (packet_type == DMX_PES_PACKET)
-		info = &(meta_data->info.pes.pts_dts_info);
-	else
-		info = &(meta_data->info.framing.pts_dts_info);
-
-	if (feed_data->write_pts_dts) {
-		if ((pes_header->pts_dts_flag == 2) ||
-			(pes_header->pts_dts_flag == 3)) {
-			info->pts_exist = 1;
-
-			info->pts =
-				((u64)pes_header->pts_1 << 30) |
-				((u64)pes_header->pts_2 << 22) |
-				((u64)pes_header->pts_3 << 15) |
-				((u64)pes_header->pts_4 << 7) |
-				(u64)pes_header->pts_5;
-		} else {
-			info->pts_exist = 0;
-			info->pts = 0;
-		}
-
-		if (pes_header->pts_dts_flag == 3) {
-			info->dts_exist = 1;
-
-			info->dts =
-				((u64)pes_header->dts_1 << 30) |
-				((u64)pes_header->dts_2 << 22) |
-				((u64)pes_header->dts_3 << 15) |
-				((u64)pes_header->dts_4 << 7) |
-				(u64)pes_header->dts_5;
-		} else {
-			info->dts_exist = 0;
-			info->dts = 0;
-		}
-	} else {
-		info->pts_exist = 0;
-		info->dts_exist = 0;
-	}
-}
-
 static int mpq_dmx_process_video_packet_framing(
 			struct dvb_demux_feed *feed,
 			const u8 *buf)
@@ -1550,7 +1611,6 @@
 			feed_data->pes_header_offset = 0;
 			feed_data->pes_header_left_bytes =
 				PES_MANDATORY_FIELDS_LEN;
-			feed_data->write_pts_dts = 0;
 		} else {
 			feed->pusi_seen = 1;
 		}
@@ -1692,6 +1752,8 @@
 		feed->peslen += bytes_avail;
 
 		meta_data.packet_type = DMX_FRAMING_INFO_PACKET;
+		packet.raw_data_handle = feed_data->buffer_desc.handle;
+		packet.raw_data_offset = 0;
 		packet.user_data_len =
 				sizeof(struct mpq_adapter_video_meta_data);
 
@@ -1701,10 +1763,10 @@
 					feed->indexing_params.standard,
 					feed_data->last_framing_match_type);
 				if (is_video_frame == 1) {
-					mpq_dmx_get_pts_dts(feed_data,
-						pes_header,
-						&meta_data,
-						DMX_FRAMING_INFO_PACKET);
+					mpq_dmx_write_pts_dts(feed_data,
+						&(meta_data.info.framing.
+							pts_dts_info));
+					mpq_dmx_save_pts_dts(feed_data);
 				} else {
 					meta_data.info.framing.
 						pts_dts_info.pts_exist = 0;
@@ -1717,8 +1779,6 @@
 				 */
 				meta_data.info.framing.pattern_type =
 					feed_data->last_framing_match_type;
-				packet.raw_data_addr =
-					feed_data->last_framing_match_address;
 
 				pattern_addr = feed_data->pes_payload_address +
 					framing_res.info[i].offset -
@@ -1742,10 +1802,8 @@
 					  feed_data->first_pattern_offset;
 				}
 
-				MPQ_DVB_DBG_PRINT("Writing Packet: "
-					"addr = 0x%X, len = %d, type = %d, "
-					"isPts = %d, isDts = %d\n",
-					packet.raw_data_addr,
+				MPQ_DVB_DBG_PRINT(
+					"Writing Packet: len = %d, type = %d, isPts = %d, isDts = %d\n",
 					packet.raw_data_len,
 					meta_data.info.framing.pattern_type,
 					meta_data.info.framing.
@@ -1754,16 +1812,11 @@
 						pts_dts_info.dts_exist);
 
 				if (mpq_streambuffer_pkt_write(stream_buffer,
-						&packet,
-						(u8 *)&meta_data) < 0) {
-							MPQ_DVB_ERR_PRINT(
-								"%s: "
-								"Couldn't write packet. "
-								"Should never happen\n",
+					&packet,
+					(u8 *)&meta_data) < 0) {
+						MPQ_DVB_ERR_PRINT(
+							"%s: Couldn't write packet. Should never happen\n",
 								__func__);
-				} else {
-					if (is_video_frame == 1)
-						feed_data->write_pts_dts = 0;
 				}
 			}
 
@@ -1855,18 +1908,17 @@
 			 */
 
 			if (0 == feed_data->pes_header_left_bytes) {
-				packet.raw_data_addr =
-					feed_data->pes_payload_address;
-
 				packet.raw_data_len = feed->peslen;
-
+				packet.raw_data_handle =
+					feed_data->buffer_desc.handle;
+				packet.raw_data_offset = 0;
 				packet.user_data_len =
 					sizeof(struct
 						mpq_adapter_video_meta_data);
 
-				mpq_dmx_get_pts_dts(feed_data, pes_header,
-							&meta_data,
-							DMX_PES_PACKET);
+				mpq_dmx_write_pts_dts(feed_data,
+					&(meta_data.info.pes.pts_dts_info));
+				mpq_dmx_save_pts_dts(feed_data);
 
 				meta_data.packet_type = DMX_PES_PACKET;
 
@@ -1879,8 +1931,6 @@
 						"Couldn't write packet. "
 						"Should never happen\n",
 						__func__);
-				else
-					feed_data->write_pts_dts = 0;
 			} else {
 				MPQ_DVB_ERR_PRINT(
 					"%s: received PUSI"
diff --git a/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.h b/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.h
index 69305b6..f7af1ef 100644
--- a/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.h
+++ b/drivers/media/dvb/mpq/demux/mpq_dmx_plugin_common.h
@@ -275,6 +275,7 @@
  * @plugin_data: Underlying plugin's own private data.
  * @video_buffer: Holds the streamer buffer shared with
  * the decoder for feeds having the data going to the decoder.
+ * @buffer_desc: Holds decoder buffer(s) information used for stream buffer.
  * @pes_header: Used for feeds that output data to decoder,
  * holds PES header of current processed PES.
  * @pes_header_left_bytes: Used for feeds that output data to decoder,
@@ -308,14 +309,18 @@
  * to the stream buffer.
  * @first_prefix_size: used to save the prefix size used to find the first
  * pattern written to the stream buffer.
- * @write_pts_dts: Flag used to decide if to write PTS/DTS information
- * (if it is available in the PES header) in the meta-data passed
- * to the video decoder. PTS/DTS information is written in the first
- * packet after it is available.
+ * @saved_pts_dts_info: used to save PTS/DTS information until it is written.
+ * @new_pts_dts_info: used to store PTS/DTS information from current PES header.
+ * @saved_info_used: indicates if saved PTS/DTS information was used.
+ * @new_info_exists: indicates if new PTS/DTS information exists in
+ * new_pts_dts_info that should be saved to saved_pts_dts_info.
+ * @first_pts_dts_copy: a flag used to indicate if PTS/DTS information needs
+ * to be copied from the currently parsed PES header to the saved_pts_dts_info.
  */
 struct mpq_video_feed_info {
 	void *plugin_data;
 	struct mpq_streambuffer *video_buffer;
+	struct mpq_streambuffer_buffer_desc buffer_desc;
 	struct pes_packet_header pes_header;
 	u32 pes_header_left_bytes;
 	u32 pes_header_offset;
@@ -331,7 +336,11 @@
 	struct mpq_framing_prefix_size_masks prefix_size;
 	u32 first_pattern_offset;
 	u32 first_prefix_size;
-	int write_pts_dts;
+	struct dmx_pts_dts_info saved_pts_dts_info;
+	struct dmx_pts_dts_info new_pts_dts_info;
+	int saved_info_used;
+	int new_info_exists;
+	int first_pts_dts_copy;
 };
 
 /**
diff --git a/drivers/media/dvb/mpq/include/mpq_stream_buffer.h b/drivers/media/dvb/mpq/include/mpq_stream_buffer.h
index 4ea4222..9476c73 100644
--- a/drivers/media/dvb/mpq/include/mpq_stream_buffer.h
+++ b/drivers/media/dvb/mpq/include/mpq_stream_buffer.h
@@ -19,7 +19,7 @@
 /**
  * DOC: MPQ Stream Buffer
  *
- * A stream buffer implmenetation used to transfer data between two units
+ * A stream buffer implementation is used to transfer data between two units
  * such as demux and decoders. The implementation relies on dvb_ringbuffer
  * implementation. Refer to dvb_ringbuffer.h for details.
  *
@@ -28,8 +28,19 @@
  * meta-data (information from PES header for example).
  *
  * The meta-data uses dvb_ringbuffer packet interface. Each meta-data
- * packet hold the address and size of raw-data described by the
- * meta-data packet, in addition to user's own parameters if any required.
+ * packet points to the data buffer, and includes the offset to the data in the
+ * buffer, the size of raw-data described by the meta-data packet, and also the
+ * size of user's own parameters if any required.
+ *
+ * Data can be managed in two ways: ring-buffer & linear buffers, as specified
+ * in initialization when calling the mpq_streambuffer_init function.
+ * For managing data as a ring buffer exactly 1 data buffer descriptor must be
+ * specified in initialization. For this mode, dvb_ringbuffer is used "as-is".
+ * For managing data in several linear buffers, an array of buffer descriptors
+ * must be passed.
+ * For both modes, data descriptor(s) must be remain valid throughout the life
+ * span of the mpq_streambuffer object.
+ * Apart from initialization API remains the same for both modes.
  *
  * Contrary to dvb_ringbuffer implementation, this API makes sure there's
  * enough data to read/write when making read/write operations.
@@ -44,18 +55,22 @@
  *
  * Typical call flow from producer:
  *
- * - Start writting the raw-data of new packet, the following call is
+ * - Start writing the raw-data of new packet, the following call is
  *   repeated until end of data of the specific packet
  *
- *     mpq_streambuffer_data_write(...)
+ *      mpq_streambuffer_data_write(...)
  *
  * - Now write a new packet describing the new available raw-data
- *     mpq_streambuffer_pkt_write(...)
+ *      mpq_streambuffer_pkt_write(...)
+ *
+ *   For linear buffer mode, writing a new packet with data size > 0, causes the
+ *   current buffer to be marked as pending for reading, and triggers moving to
+ *   the next available buffer, that shall now be the current write buffer.
  *
  * Typical call flow from consumer:
  *
  * - Poll for next available packet:
- *      mpq_streambuffer_pkt_next(&streambuff,-1)
+ *      mpq_streambuffer_pkt_next(&streambuff,-1,&len)
  *
  *   In different approach, consumer can wait on event for new data and then
  *   call mpq_streambuffer_pkt_next, waiting for data can be done as follows:
@@ -77,58 +92,126 @@
  *      data buffer, the amount of raw-data is provided part of the
  *      packet's information. User should then call mpq_streambuffer_pkt_dispose
  *      with dispose_data set to 0 as the raw-data was already disposed.
+ *      Note that secure buffer cannot be accessed directly and an error will
+ *      occur.
  *
  *   2. Access the data directly using the raw-data address. The address
  *      of the raw data is provided part of the packet's information. User
  *      then should call mpq_streambuffer_pkt_dispose with dispose_data set
  *      to 1 to dispose the packet along with it's raw-data.
+ *
+ * - Disposal of packets:
+ *      mpq_streambuffer_pkt_dispose(...)
+ *
+ *   For linear buffer mode, disposing of a packet with data size > 0, causes
+ *   the current buffer to be marked as free for writing, and triggers moving to
+ *   the next available buffer, that shall now be the current read buffer.
+
+ *
  */
 
+struct mpq_streambuffer;
+
+typedef void (*mpq_streambuffer_pkt_dispose_cb) (
+	struct mpq_streambuffer *sbuff,
+	void *user_data);
+
+enum mpq_streambuffer_mode {
+	MPQ_STREAMBUFFER_BUFFER_MODE_RING,
+	MPQ_STREAMBUFFER_BUFFER_MODE_LINEAR
+};
+
 /**
  * struct mpq_streambuffer - mpq stream buffer representation
  *
- * @raw_data: The buffer used to hold the raw-data
+ * @raw_data: The buffer used to hold raw-data, or linear buffer descriptors
  * @packet_data: The buffer user to hold the meta-data
+ * @buffers: array of buffer descriptor(s) holding buffer initial & dynamic
+ *	     buffer information
+ * @mode: mpq_streambuffer buffer management work mode - Ring-buffer or Linear
+ *	  buffers
+ * @buffers_num: number of data buffers to manage
+ * @pending_buffers_count: for linear buffer management, counts the number of
+ * buffer that has been
  */
 struct mpq_streambuffer {
 	struct dvb_ringbuffer raw_data;
 	struct dvb_ringbuffer packet_data;
+	struct mpq_streambuffer_buffer_desc *buffers;
+	enum mpq_streambuffer_mode mode;
+	u32 buffers_num;
+	u32 pending_buffers_count;
+	mpq_streambuffer_pkt_dispose_cb cb;
+	void *cb_user_data;
+};
+
+/**
+ * mpq_streambuffer_linear_desc
+ * @handle:	ION handle's file descriptor of buffer
+ * @base:	kernel mapped address to start of buffer.
+ *		Can be NULL for secured buffers
+ * @size:	size of buffer
+ * @read_ptr:	initial read pointer value (should normally be 0)
+ * @write_ptr:	initial write pointer value (should normally be 0)
+ */
+struct mpq_streambuffer_buffer_desc {
+	int	handle;
+	void	*base;
+	u32	size;
+	u32	read_ptr;
+	u32	write_ptr;
 };
 
 /**
  * struct mpq_streambuffer_packet_header - packet header saved in packet buffer
  * @user_data_len: length of private user (meta) data
- * @raw_data_addr: raw-data address in the raw-buffer described by the packet
+ * @raw_data_handle: ION handle's file descriptor of raw-data buffer
+ * @raw_data_offset: offset of raw-data from start of buffer (0 for linear)
  * @raw_data_len: size of raw-data in the raw-data buffer (can be 0)
  *
  * The packet structure that is saved in each packet-buffer:
  * user_data_len
- * raw_data_addr
+ * raw_data_handle
+ * raw_data_offset
  * raw_data_len
  * private user-data bytes
  */
 struct mpq_streambuffer_packet_header {
 	u32 user_data_len;
-	u32	raw_data_addr;
-	u32	raw_data_len;
+	int raw_data_handle;
+	u32 raw_data_offset;
+	u32 raw_data_len;
 } __packed;
 
 /**
  * mpq_streambuffer_init - Initialize a new stream buffer
  *
  * @sbuff: The buffer to initialize
- * @data_buff: The buffer holding raw-data
- * @data_buff_len: Size of raw-data buffer
+ * @data_buffers: array of data buffer descriptor(s).
+ *		  Data descriptor(s) must be remain valid throughout the life
+ *		  span of the mpq_streambuffer object
+ * @data_buff_num: number of data buffer in array
  * @packet_buff: The buffer holding meta-data
  * @packet_buff_size: Size of meta-data buffer
+ *
+ * Return	Error status, -EINVAL if any of the arguments are invalid
+ *
+ * Note:
+ * for data_buff_num > 1, mpq_streambuffer object manages these buffers as a
+ * separated set of linear buffers. A linear buffer cannot wrap-around and one
+ * can only write as many data bytes as the buffer's size. Data will not be
+ * written to the next free buffer.
  */
-void mpq_streambuffer_init(
+int mpq_streambuffer_init(
 		struct mpq_streambuffer *sbuff,
-		void *data_buff, size_t data_buff_len,
-		void *packet_buff, size_t packet_buff_size);
+		enum mpq_streambuffer_mode mode,
+		struct mpq_streambuffer_buffer_desc *data_buffers,
+		u32 data_buff_num,
+		void *packet_buff,
+		size_t packet_buff_size);
 
 /**
- * mpq_streambuffer_packet_next - Returns index of next avaialble packet.
+ * mpq_streambuffer_packet_next - Returns index of next available packet.
  *
  * @sbuff: The stream buffer
  * @idx: Previous packet index or -1 to return index of the the first
@@ -234,19 +317,29 @@
  * @buf: The buffer to read the raw-data data to
  * @len: The length of the buffer that will hold the raw-data
  *
- * Return  The actual number of bytes read
+ * Return  The actual number of bytes read or error code
  *
- * This fucntion copies the data from the ring-buffer to the
+ * This function copies the data from the ring-buffer to the
  * provided buf parameter. The user can save the extra copy by accessing
  * the data pointer directly and reading from it, then update the
  * read pointer by the amount of data that was read using
  * mpq_streambuffer_data_read_dispose
  */
-size_t mpq_streambuffer_data_read(
+ssize_t mpq_streambuffer_data_read(
 		struct mpq_streambuffer *sbuff,
 		u8 *buf, size_t len);
 
 /**
+ * mpq_streambuffer_data_read_user
+ *
+ * Same as mpq_streambuffer_data_read except data can be copied to user-space
+ * buffer.
+ */
+ssize_t mpq_streambuffer_data_read_user(
+		struct mpq_streambuffer *sbuff,
+		u8 __user *buf, size_t len);
+
+/**
  * mpq_streambuffer_data_read_dispose - Advances the raw-buffer read pointer.
  * Assumes the raw-data was read by the user directly.
  *
@@ -256,12 +349,69 @@
  * Return  error status, -EINVAL if buffer there's no enough data to
  *			be disposed
  *
- * The user can instead dipose a packet along with the data in the
+ * The user can instead dispose a packet along with the data in the
  * raw-data buffer using mpq_streambuffer_pkt_dispose.
  */
 int mpq_streambuffer_data_read_dispose(
 		struct mpq_streambuffer *sbuff,
 		size_t len);
+/**
+ * mpq_streambuffer_get_buffer_handle - Returns the current linear buffer
+ * ION handle.
+ * @sbuff: The stream buffer
+ * @read_buffer: specifies if a read buffer handle is requested (when set),
+ *		 or a write buffer handle is requested.
+ *		 For linear buffer mode read & write buffers may be different
+ *		 buffers. For ring buffer mode, the same (single) buffer handle
+ *		 is returned.
+ * buffer handle
+ * @handle: returned handle
+ *
+ * Return error status
+ * -EINVAL is arguments are invalid.
+ * -EPERM if stream buffer specified was not initialized with linear support.
+ */
+int mpq_streambuffer_get_buffer_handle(
+	struct mpq_streambuffer *sbuff,
+	int read_buffer,
+	int *handle);
+
+/**
+ * mpq_streambuffer_data_free - Returns number of free bytes in data buffer.
+ * @sbuff: The stream buffer object
+ *
+ * Note: for linear buffer management this return number of free bytes in the
+ * current write buffer only.
+ */
+ssize_t mpq_streambuffer_data_free(
+	struct mpq_streambuffer *sbuff);
+
+/**
+ * mpq_streambuffer_data_avail - Returns number of bytes in data buffer that
+ * can be read.
+ * @sbuff: The stream buffer object
+ *
+ * Note: for linear buffer management this return number of data bytes in the
+ * current read buffer only.
+ */
+ssize_t mpq_streambuffer_data_avail(
+	struct mpq_streambuffer *sbuff);
+
+/**
+ * mpq_streambuffer_register_pkt_dispose - Registers a callback to notify on
+ * packet disposal events.
+ * can be read.
+ * @sbuff: The stream buffer object
+ * @cb_func: user callback function
+ * @user_data: user data to be passed to callback function.
+ *
+ * Returns error status
+ * -EINVAL if arguments are invalid
+ */
+int mpq_streambuffer_register_pkt_dispose(
+	struct mpq_streambuffer *sbuff,
+	mpq_streambuffer_pkt_dispose_cb cb_func,
+	void *user_data);
 
 
 
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index d2ed996..f33122f 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -35,6 +35,17 @@
 	   To compile this driver as a module, choose M here: the module will
 	   be called user-rc-input.
 
+config USER_SP_RC_INPUT
+	tristate "User Space Input device wrapper for Standby Processor Remote Control"
+	depends on RC_CORE
+
+	---help---
+	   Say Y if you want to report remote control input events
+	   from userspace.
+
+	   To compile this driver as a module, choose M here: the module will
+	   be called user-sp-input.
+
 source "drivers/media/rc/keymaps/Kconfig"
 
 config IR_NEC_DECODER
diff --git a/drivers/media/rc/Makefile b/drivers/media/rc/Makefile
index be02eec..fdc3e99 100644
--- a/drivers/media/rc/Makefile
+++ b/drivers/media/rc/Makefile
@@ -14,6 +14,7 @@
 obj-$(CONFIG_IR_MCE_KBD_DECODER) += ir-mce_kbd-decoder.o
 obj-$(CONFIG_IR_LIRC_CODEC) += ir-lirc-codec.o
 obj-$(CONFIG_USER_RC_INPUT) += user-rc-input.o
+obj-$(CONFIG_USER_SP_RC_INPUT) += user-sp-rc-input.o
 
 # stand-alone IR receivers/transmitters
 obj-$(CONFIG_RC_ATI_REMOTE) += ati_remote.o
diff --git a/drivers/media/rc/user-sp-rc-input.c b/drivers/media/rc/user-sp-rc-input.c
new file mode 100644
index 0000000..83da95d
--- /dev/null
+++ b/drivers/media/rc/user-sp-rc-input.c
@@ -0,0 +1,246 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/ioctl.h>
+#include <linux/fs.h>
+#include <linux/uaccess.h>
+#include <linux/platform_device.h>
+#include <linux/device.h>
+#include <linux/cdev.h>
+#include <linux/slab.h>
+
+#include <media/rc-core.h>
+#include <media/user-rc-input.h>
+
+#define MAX_SP_DEVICES		1
+#define USER_SP_INPUT_DEV_NAME	"user-sp-input"
+#define USER_SP_INPUT_DRV_NAME	"sp-user-input"
+
+struct user_sp_input_dev {
+	struct cdev sp_input_cdev;
+	struct class *sp_input_class;
+	struct device *sp_input_dev;
+	struct rc_dev *spdev;
+	dev_t sp_input_base_dev;
+	struct device *dev;
+	int in_use;
+};
+
+static int user_sp_input_open(struct inode *inode, struct file *file)
+{
+	struct cdev *input_cdev = inode->i_cdev;
+	struct user_sp_input_dev *input_dev =
+	container_of(input_cdev, struct user_sp_input_dev, sp_input_cdev);
+
+	if (input_dev->in_use) {
+		dev_err(input_dev->dev,
+		"Device is already open..only one instance is allowed\n");
+		return -EBUSY;
+	}
+	input_dev->in_use++;
+	file->private_data = input_dev;
+
+	return 0;
+}
+
+static int user_sp_input_release(struct inode *inode, struct file *file)
+{
+	struct user_sp_input_dev *input_dev = file->private_data;
+
+	input_dev->in_use--;
+
+	return 0;
+}
+
+static ssize_t user_sp_input_write(struct file *file,
+		const char __user *buffer, size_t count, loff_t *ppos)
+{
+	int ret = count;
+	struct user_sp_input_dev *input_dev = file->private_data;
+	unsigned char cmd = 0;
+	int scancode = 0;
+
+	if (copy_from_user(&cmd, buffer, 1)) {
+		dev_err(input_dev->dev, "Copy from user failed\n");
+		ret = -EFAULT;
+		goto out_free;
+	}
+
+	if (copy_from_user(&scancode, &buffer[1], 4)) {
+		dev_err(input_dev->dev, "Copy from user failed\n");
+		ret = -EFAULT;
+		goto out_free;
+	}
+
+	switch (cmd) {
+	case USER_CONTROL_PRESSED:
+		dev_dbg(input_dev->dev, "user controlled pressed 0x%x\n",
+			scancode);
+		rc_keydown(input_dev->spdev, scancode, 0);
+		break;
+	case USER_CONTROL_REPEATED:
+		dev_dbg(input_dev->dev, "user controlled repeated 0x%x\n",
+			scancode);
+		rc_repeat(input_dev->spdev);
+		break;
+	case USER_CONTROL_RELEASED:
+		dev_dbg(input_dev->dev, "user controlled released 0x%x\n",
+			scancode);
+		rc_keyup(input_dev->spdev);
+		break;
+	}
+
+out_free:
+	return ret;
+}
+
+const struct file_operations sp_fops = {
+	.owner  = THIS_MODULE,
+	.open   = user_sp_input_open,
+	.write  = user_sp_input_write,
+	.release = user_sp_input_release,
+};
+
+static int __devinit user_sp_input_probe(struct platform_device *pdev)
+{
+	struct user_sp_input_dev *user_sp_dev;
+	struct rc_dev *spdev;
+	int retval;
+
+	user_sp_dev = kzalloc(sizeof(struct user_sp_input_dev), GFP_KERNEL);
+	if (!user_sp_dev)
+		return -ENOMEM;
+
+	user_sp_dev->sp_input_class = class_create(THIS_MODULE,
+						"user-sp-input-loopback");
+
+	if (IS_ERR(user_sp_dev->sp_input_class)) {
+		retval = PTR_ERR(user_sp_dev->sp_input_class);
+		goto err;
+	}
+
+	retval = alloc_chrdev_region(&user_sp_dev->sp_input_base_dev, 0,
+				MAX_SP_DEVICES, USER_SP_INPUT_DEV_NAME);
+
+	if (retval) {
+		dev_err(&pdev->dev,
+			"alloc_chrdev_region failed\n");
+		goto alloc_chrdev_err;
+	}
+
+	dev_info(&pdev->dev, "User space report standby key event input" \
+				" driver registered, major %d\n",
+				MAJOR(user_sp_dev->sp_input_base_dev));
+
+	cdev_init(&user_sp_dev->sp_input_cdev, &sp_fops);
+	retval = cdev_add(&user_sp_dev->sp_input_cdev,
+			user_sp_dev->sp_input_base_dev, MAX_SP_DEVICES);
+	if (retval) {
+		dev_err(&pdev->dev, "cdev_add failed\n");
+		goto cdev_add_err;
+	}
+	user_sp_dev->sp_input_dev = device_create(user_sp_dev->sp_input_class,
+		NULL, MKDEV(MAJOR(user_sp_dev->sp_input_base_dev), 0), NULL,
+		"user-sp-input-dev%d", 0);
+
+	if (IS_ERR(user_sp_dev->sp_input_dev)) {
+		retval = PTR_ERR(user_sp_dev->sp_input_dev);
+		dev_err(&pdev->dev, "device_create failed\n");
+		goto device_create_err;
+	}
+
+	spdev = rc_allocate_device();
+	if (!spdev) {
+		dev_err(&pdev->dev, "failed to allocate rc device");
+		retval = -ENOMEM;
+		goto err_allocate_device;
+	}
+
+	spdev->driver_type = RC_DRIVER_SCANCODE;
+	spdev->allowed_protos = RC_TYPE_OTHER;
+	spdev->input_name = USER_SP_INPUT_DEV_NAME;
+	spdev->input_id.bustype = BUS_HOST;
+	spdev->driver_name = USER_SP_INPUT_DRV_NAME;
+	spdev->map_name = RC_MAP_RC6_PHILIPS;
+
+	retval = rc_register_device(spdev);
+	if (retval < 0) {
+		dev_err(&pdev->dev, "failed to register rc device\n");
+		goto rc_register_err;
+	}
+	user_sp_dev->spdev = spdev;
+	user_sp_dev->dev = &pdev->dev;
+	platform_set_drvdata(pdev, user_sp_dev);
+	user_sp_dev->in_use = 0;
+
+	return 0;
+
+rc_register_err:
+	rc_free_device(spdev);
+err_allocate_device:
+	device_destroy(user_sp_dev->sp_input_class,
+			MKDEV(MAJOR(user_sp_dev->sp_input_base_dev), 0));
+cdev_add_err:
+	unregister_chrdev_region(user_sp_dev->sp_input_base_dev,
+				MAX_SP_DEVICES);
+device_create_err:
+	cdev_del(&user_sp_dev->sp_input_cdev);
+alloc_chrdev_err:
+	class_destroy(user_sp_dev->sp_input_class);
+err:
+	kfree(user_sp_dev);
+	return retval;
+}
+
+static int __devexit user_sp_input_remove(struct platform_device *pdev)
+{
+	struct user_sp_input_dev *user_sp_dev = platform_get_drvdata(pdev);
+
+	platform_set_drvdata(pdev, NULL);
+	rc_free_device(user_sp_dev->spdev);
+	device_destroy(user_sp_dev->sp_input_class,
+			MKDEV(MAJOR(user_sp_dev->sp_input_base_dev), 0));
+	unregister_chrdev_region(user_sp_dev->sp_input_base_dev,
+				MAX_SP_DEVICES);
+	cdev_del(&user_sp_dev->sp_input_cdev);
+	class_destroy(user_sp_dev->sp_input_class);
+	kfree(user_sp_dev);
+
+	return 0;
+}
+
+static struct platform_driver user_sp_input_driver = {
+	.probe  = user_sp_input_probe,
+	.remove = __devexit_p(user_sp_input_remove),
+	.driver = {
+		.name   = USER_SP_INPUT_DRV_NAME,
+		.owner  = THIS_MODULE,
+	},
+};
+
+static int __init user_sp_input_init(void)
+{
+	return platform_driver_register(&user_sp_input_driver);
+}
+module_init(user_sp_input_init);
+
+static void __exit user_sp_input_exit(void)
+{
+	platform_driver_unregister(&user_sp_input_driver);
+}
+module_exit(user_sp_input_exit);
+
+MODULE_DESCRIPTION("User SP RC Input driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/video/msm/server/msm_cam_server.c b/drivers/media/video/msm/server/msm_cam_server.c
index f61b74f..af31748 100644
--- a/drivers/media/video/msm/server/msm_cam_server.c
+++ b/drivers/media/video/msm/server/msm_cam_server.c
@@ -1445,9 +1445,10 @@
 					/*so that it isn't closed again*/
 					pmctl->mctl_release = NULL;
 				}
-				msm_cam_server_send_error_evt(pmctl,
-					V4L2_EVENT_PRIVATE_START +
-					MSM_CAM_APP_NOTIFY_ERROR_EVENT);
+				if (pmctl)
+					msm_cam_server_send_error_evt(pmctl,
+						V4L2_EVENT_PRIVATE_START +
+						MSM_CAM_APP_NOTIFY_ERROR_EVENT);
 			}
 		}
 		sub.type = V4L2_EVENT_ALL;
@@ -1753,7 +1754,7 @@
 	case NOTIFY_VFE_MSG_COMP_STATS:
 	case NOTIFY_VFE_BUF_EVT:
 		p_mctl = msm_cam_server_get_mctl(mctl_handle);
-		if (p_mctl->isp_notify && p_mctl->vfe_sdev)
+		if (p_mctl && p_mctl->isp_notify && p_mctl->vfe_sdev)
 			rc = p_mctl->isp_notify(p_mctl,
 				p_mctl->vfe_sdev, notification, arg);
 		break;
@@ -1772,18 +1773,20 @@
 		break;
 	case NOTIFY_AXI_RDI_SOF_COUNT:
 		p_mctl = msm_cam_server_get_mctl(mctl_handle);
-		if (p_mctl->axi_sdev)
+		if (p_mctl && p_mctl->axi_sdev)
 			rc = v4l2_subdev_call(p_mctl->axi_sdev, core, ioctl,
 				VIDIOC_MSM_AXI_RDI_COUNT_UPDATE, arg);
 		break;
 	case NOTIFY_PCLK_CHANGE:
 		p_mctl = v4l2_get_subdev_hostdata(sd);
-		if (p_mctl->axi_sdev)
-			rc = v4l2_subdev_call(p_mctl->axi_sdev, video,
-			s_crystal_freq, *(uint32_t *)arg, 0);
-		else
-			rc = v4l2_subdev_call(p_mctl->vfe_sdev, video,
-			s_crystal_freq, *(uint32_t *)arg, 0);
+		if (p_mctl) {
+			if (p_mctl->axi_sdev)
+				rc = v4l2_subdev_call(p_mctl->axi_sdev, video,
+				s_crystal_freq, *(uint32_t *)arg, 0);
+			else
+				rc = v4l2_subdev_call(p_mctl->vfe_sdev, video,
+				s_crystal_freq, *(uint32_t *)arg, 0);
+		}
 		break;
 	case NOTIFY_GESTURE_EVT:
 		rc = v4l2_subdev_call(g_server_dev.gesture_device,
@@ -1795,8 +1798,10 @@
 		break;
 	case NOTIFY_VFE_CAMIF_ERROR: {
 		p_mctl = msm_cam_server_get_mctl(mctl_handle);
-		msm_cam_server_send_error_evt(p_mctl, V4L2_EVENT_PRIVATE_START
-			+ MSM_CAM_APP_NOTIFY_ERROR_EVENT);
+		if (p_mctl)
+			msm_cam_server_send_error_evt(p_mctl,
+				V4L2_EVENT_PRIVATE_START +
+				MSM_CAM_APP_NOTIFY_ERROR_EVENT);
 		break;
 	}
 	default:
diff --git a/drivers/media/video/msm_vidc/Makefile b/drivers/media/video/msm_vidc/Makefile
index 9b3af9d..2a1f40f 100644
--- a/drivers/media/video/msm_vidc/Makefile
+++ b/drivers/media/video/msm_vidc/Makefile
@@ -5,6 +5,5 @@
 				msm_venc.o \
 				msm_smem.o \
 				msm_vidc_debug.o \
-				msm_vidc_ssr.o \
 				vidc_hal.o \
 				vidc_hal_interrupt_handler.o \
diff --git a/drivers/media/video/msm_vidc/msm_smem.c b/drivers/media/video/msm_vidc/msm_smem.c
index e1b73ef..eae18c4 100644
--- a/drivers/media/video/msm_vidc/msm_smem.c
+++ b/drivers/media/video/msm_vidc/msm_smem.c
@@ -134,6 +134,9 @@
 	}
 
 	heap_mask = ION_HEAP(ION_CP_MM_HEAP_ID);
+	if (!(flags & SMEM_SECURE))
+		heap_mask |= ION_HEAP(ION_IOMMU_HEAP_ID);
+
 	dprintk(VIDC_DBG, "domain: %d, partition: %d\n",
 		domain, partition);
 	hndl = ion_alloc(client->clnt, size, align, heap_mask, ionflags);
diff --git a/drivers/media/video/msm_vidc/msm_v4l2_vidc.c b/drivers/media/video/msm_vidc/msm_v4l2_vidc.c
index 38e8de1..0979e99 100644
--- a/drivers/media/video/msm_vidc/msm_v4l2_vidc.c
+++ b/drivers/media/video/msm_vidc/msm_v4l2_vidc.c
@@ -92,11 +92,10 @@
 		.src = MSM_BUS_MASTER_VIDEO_P0_OCMEM,
 		.dst = MSM_BUS_SLAVE_OCMEM,
 		.ab = 3760000000U,
-		.ib = 3910400000U,
+		.ib = 4888000000ULL,
 	},
 };
 
-
 static struct msm_bus_vectors dec_ocmem_init_vectors[]  = {
 	{
 		.src = MSM_BUS_MASTER_VIDEO_P0_OCMEM,
@@ -147,7 +146,7 @@
 		.src = MSM_BUS_MASTER_VIDEO_P0_OCMEM,
 		.dst = MSM_BUS_SLAVE_OCMEM,
 		.ab = 2767360000U,
-		.ib = 3113280000U,
+		.ib = 4981248000ULL,
 	},
 };
 
@@ -156,7 +155,7 @@
 		.src = MSM_BUS_MASTER_VIDEO_P0_OCMEM,
 		.dst = MSM_BUS_SLAVE_OCMEM,
 		.ab = 3459200000U,
-		.ib = 3459200000U,
+		.ib = 6226560000ULL,
 	},
 };
 
@@ -295,7 +294,7 @@
 		.src = MSM_BUS_MASTER_VIDEO_P0,
 		.dst = MSM_BUS_SLAVE_EBI_CH0,
 		.ab = 161200000,
-		.ib = 2659800000U,
+		.ib = 6400000000ULL,
 	},
 };
 
@@ -358,7 +357,7 @@
 		.src = MSM_BUS_MASTER_VIDEO_P0,
 		.dst = MSM_BUS_SLAVE_EBI_CH0,
 		.ab = 2020000000U,
-		.ib = 3636000000U,
+		.ib = 6400000000ULL,
 	},
 };
 
diff --git a/drivers/media/video/msm_vidc/msm_vdec.c b/drivers/media/video/msm_vidc/msm_vdec.c
index 711b3007..440c704 100644
--- a/drivers/media/video/msm_vidc/msm_vdec.c
+++ b/drivers/media/video/msm_vidc/msm_vdec.c
@@ -22,11 +22,15 @@
 #define MSM_VDEC_DVC_NAME "msm_vdec_8974"
 #define DEFAULT_HEIGHT 720
 #define DEFAULT_WIDTH 1280
-#define MAX_SUPPORTED_WIDTH 1920
-#define MAX_SUPPORTED_HEIGHT 1088
+#define MAX_SUPPORTED_WIDTH 3820
+#define MAX_SUPPORTED_HEIGHT 2160
 #define MIN_NUM_OUTPUT_BUFFERS 4
 #define MAX_NUM_OUTPUT_BUFFERS 6
 
+enum msm_vdec_ctrl_cluster {
+	MSM_VDEC_CTRL_CLUSTER_MAX = 1,
+};
+
 static const char *const mpeg_video_vidc_divx_format[] = {
 	"DIVX Format 3",
 	"DIVX Format 4",
@@ -68,7 +72,7 @@
 	"Extradata aspect ratio",
 };
 
-static const struct msm_vidc_ctrl msm_vdec_ctrls[] = {
+static struct msm_vidc_ctrl msm_vdec_ctrls[] = {
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_STREAM_FORMAT,
 		.name = "NAL Format",
@@ -85,6 +89,7 @@
 		),
 		.qmenu = mpeg_video_stream_format,
 		.step = 0,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_OUTPUT_ORDER,
@@ -99,6 +104,7 @@
 			),
 		.qmenu = mpeg_video_output_order,
 		.step = 0,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_ENABLE_PICTURE_TYPE,
@@ -110,6 +116,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_KEEP_ASPECT_RATIO,
@@ -121,6 +128,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_POST_LOOP_DEBLOCKER_MODE,
@@ -132,6 +140,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_DIVX_FORMAT,
@@ -147,6 +156,7 @@
 			),
 		.qmenu = mpeg_video_vidc_divx_format,
 		.step = 0,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_MB_ERROR_MAP_REPORTING,
@@ -158,6 +168,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_CONTINUE_DATA_TRANSFER,
@@ -169,6 +180,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_SYNC_FRAME_DECODE,
@@ -188,6 +200,7 @@
 		.step = 0,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_EXTRADATA,
@@ -1060,10 +1073,11 @@
 	inst->prop.height = DEFAULT_HEIGHT;
 	inst->prop.width = DEFAULT_WIDTH;
 	inst->prop.fps = 30;
+	inst->prop.prev_time_stamp = 0;
 	return rc;
 }
 
-static int msm_vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
+static int try_set_ctrl(struct msm_vidc_inst *inst, struct v4l2_ctrl *ctrl)
 {
 	int rc = 0;
 	struct v4l2_control control;
@@ -1074,62 +1088,59 @@
 	enum hal_property property_id = 0;
 	u32 property_val = 0;
 	void *pdata;
-	struct msm_vidc_inst *inst = container_of(ctrl->handler,
-				struct msm_vidc_inst, ctrl_handler);
-	control.id = ctrl->id;
-	control.value = ctrl->val;
-	switch (control.id) {
+
+	switch (ctrl->id) {
 	case V4L2_CID_MPEG_VIDC_VIDEO_STREAM_FORMAT:
 		property_id =
 		HAL_PARAM_NAL_STREAM_FORMAT_SELECT;
 		stream_format.nal_stream_format_supported =
-		(0x00000001 << control.value);
+		(0x00000001 << ctrl->val);
 		pdata = &stream_format;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_OUTPUT_ORDER:
 		property_id = HAL_PARAM_VDEC_OUTPUT_ORDER;
-		property_val = control.value;
+		property_val = ctrl->val;
 		pdata = &property_val;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_ENABLE_PICTURE_TYPE:
 		property_id =
 			HAL_PARAM_VDEC_PICTURE_TYPE_DECODE;
-		enable_picture.picture_type = control.value;
+		enable_picture.picture_type = ctrl->val;
 		pdata = &enable_picture;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_KEEP_ASPECT_RATIO:
 		property_id =
 			HAL_PARAM_VDEC_OUTPUT2_KEEP_ASPECT_RATIO;
-		hal_property.enable = control.value;
+		hal_property.enable = ctrl->val;
 		pdata = &hal_property;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_POST_LOOP_DEBLOCKER_MODE:
 		property_id =
 			HAL_CONFIG_VDEC_POST_LOOP_DEBLOCKER;
-		hal_property.enable = control.value;
+		hal_property.enable = ctrl->val;
 		pdata = &hal_property;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_DIVX_FORMAT:
 		property_id = HAL_PARAM_DIVX_FORMAT;
-		property_val = control.value;
+		property_val = ctrl->val;
 		pdata = &property_val;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_MB_ERROR_MAP_REPORTING:
 		property_id =
 			HAL_CONFIG_VDEC_MB_ERROR_MAP_REPORTING;
-		hal_property.enable = control.value;
+		hal_property.enable = ctrl->val;
 		pdata = &hal_property;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_CONTINUE_DATA_TRANSFER:
 		property_id =
 			HAL_PARAM_VDEC_CONTINUE_DATA_TRANSFER;
-		hal_property.enable = control.value;
+		hal_property.enable = ctrl->val;
 		pdata = &hal_property;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_SYNC_FRAME_DECODE:
 		property_id =
 			HAL_PARAM_VDEC_SYNC_FRAME_DECODE;
-		hal_property.enable = control.value;
+		hal_property.enable = ctrl->val;
 		pdata = &hal_property;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_SECURE:
@@ -1140,7 +1151,7 @@
 	{
 		struct hal_extradata_enable extra;
 		property_id = HAL_PARAM_INDEX_EXTRADATA;
-		extra.index = msm_comm_get_hal_extradata_index(control.value);
+		extra.index = msm_comm_get_hal_extradata_index(ctrl->val);
 		extra.enable = 1;
 		pdata = &extra;
 		break;
@@ -1148,13 +1159,8 @@
 	default:
 		break;
 	}
+
 	if (property_id) {
-		rc = msm_comm_try_state(inst, MSM_VIDC_OPEN_DONE);
-		if (rc) {
-			dprintk(VIDC_ERR,
-			"Failed to move inst: %p to start done state\n", inst);
-			goto failed_open_done;
-		}
 		dprintk(VIDC_DBG,
 			"Control: HAL property=%d,ctrl_id=%d,ctrl_value=%d\n",
 			property_id,
@@ -1163,10 +1169,37 @@
 			rc = vidc_hal_session_set_property((void *)
 				inst->session, property_id,
 					pdata);
+	}
+
+	return rc;
+}
+
+static int msm_vdec_op_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	int rc = 0, c = 0;
+	struct msm_vidc_inst *inst = container_of(ctrl->handler,
+				struct msm_vidc_inst, ctrl_handler);
+	rc = msm_comm_try_state(inst, MSM_VIDC_OPEN_DONE);
+	if (rc) {
+		dprintk(VIDC_ERR,
+			"Failed to move inst: %p to start done state\n", inst);
+		goto failed_open_done;
+	}
+
+	for (c = 0; c < ctrl->ncontrols; ++c) {
+		if (ctrl->cluster[c]->is_new) {
+			rc = try_set_ctrl(inst, ctrl->cluster[c]);
+			if (rc) {
+				dprintk(VIDC_ERR, "Failed setting %x",
+						ctrl->cluster[c]->id);
+				break;
+			}
 		}
+	}
+
+failed_open_done:
 	if (rc)
 		dprintk(VIDC_ERR, "Failed to set hal property for framesize\n");
-failed_open_done:
 	return rc;
 }
 
@@ -1194,6 +1227,27 @@
 {
 	return v4l2_g_ctrl(&inst->ctrl_handler, ctrl);
 }
+
+static struct v4l2_ctrl **get_cluster(int type, int *size)
+{
+	int c = 0, sz = 0;
+	struct v4l2_ctrl **cluster = kmalloc(sizeof(struct v4l2_ctrl *) *
+			NUM_CTRLS, GFP_KERNEL);
+
+	if (type <= 0 || !size || !cluster)
+		return NULL;
+
+	for (c = 0; c < NUM_CTRLS; c++) {
+		if (msm_vdec_ctrls[c].cluster == type) {
+			cluster[sz] = msm_vdec_ctrls[c].priv;
+			++sz;
+		}
+	}
+
+	*size = sz;
+	return cluster;
+}
+
 int msm_vdec_ctrl_init(struct msm_vidc_inst *inst)
 {
 	int idx = 0;
@@ -1209,12 +1263,13 @@
 	}
 
 	for (; idx < NUM_CTRLS; idx++) {
+		struct v4l2_ctrl *ctrl = NULL;
 		if (IS_PRIV_CTRL(msm_vdec_ctrls[idx].id)) {
 			/*add private control*/
 			ctrl_cfg.def = msm_vdec_ctrls[idx].default_value;
 			ctrl_cfg.flags = 0;
 			ctrl_cfg.id = msm_vdec_ctrls[idx].id;
-			/*ctrl_cfg.is_private =
+			/* ctrl_cfg.is_private =
 			 * msm_vdec_ctrls[idx].is_private;
 			 * ctrl_cfg.is_volatile =
 			 * msm_vdec_ctrls[idx].is_volatile;*/
@@ -1228,18 +1283,19 @@
 			ctrl_cfg.type = msm_vdec_ctrls[idx].type;
 			ctrl_cfg.qmenu = msm_vdec_ctrls[idx].qmenu;
 
-			v4l2_ctrl_new_custom(&inst->ctrl_handler,
+			ctrl = v4l2_ctrl_new_custom(&inst->ctrl_handler,
 					&ctrl_cfg, NULL);
 		} else {
 			if (msm_vdec_ctrls[idx].type == V4L2_CTRL_TYPE_MENU) {
-				v4l2_ctrl_new_std_menu(&inst->ctrl_handler,
+				ctrl = v4l2_ctrl_new_std_menu(
+					&inst->ctrl_handler,
 					&msm_vdec_ctrl_ops,
 					msm_vdec_ctrls[idx].id,
 					msm_vdec_ctrls[idx].maximum,
 					msm_vdec_ctrls[idx].menu_skip_mask,
 					msm_vdec_ctrls[idx].default_value);
 			} else {
-				v4l2_ctrl_new_std(&inst->ctrl_handler,
+				ctrl = v4l2_ctrl_new_std(&inst->ctrl_handler,
 					&msm_vdec_ctrl_ops,
 					msm_vdec_ctrls[idx].id,
 					msm_vdec_ctrls[idx].minimum,
@@ -1248,11 +1304,51 @@
 					msm_vdec_ctrls[idx].default_value);
 			}
 		}
+
+
+		msm_vdec_ctrls[idx].priv = ctrl;
 	}
 	ret_val = inst->ctrl_handler.error;
 	if (ret_val)
 		dprintk(VIDC_ERR,
 			"Error adding ctrls to ctrl handle, %d\n",
 			inst->ctrl_handler.error);
+
+	/* Construct clusters */
+	for (idx = 1; idx < MSM_VDEC_CTRL_CLUSTER_MAX; ++idx) {
+		struct msm_vidc_ctrl_cluster *temp = NULL;
+		struct v4l2_ctrl **cluster = NULL;
+		int cluster_size = 0;
+
+		cluster = get_cluster(idx, &cluster_size);
+		if (!cluster || !cluster_size) {
+			dprintk(VIDC_WARN, "Failed to setup cluster of type %d",
+					idx);
+			continue;
+		}
+
+		v4l2_ctrl_cluster(cluster_size, cluster);
+
+		temp = kzalloc(sizeof(*temp), GFP_KERNEL);
+		if (!temp) {
+			ret_val = -ENOMEM;
+			break;
+		}
+
+		temp->cluster = cluster;
+		INIT_LIST_HEAD(&temp->list);
+		list_add_tail(&temp->list, &inst->ctrl_clusters);
+	}
 	return ret_val;
 }
+
+int msm_vdec_ctrl_deinit(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_ctrl_cluster *curr, *next;
+	list_for_each_entry_safe(curr, next, &inst->ctrl_clusters, list) {
+		kfree(curr->cluster);
+		kfree(curr);
+	}
+
+	return 0;
+}
diff --git a/drivers/media/video/msm_vidc/msm_vdec.h b/drivers/media/video/msm_vidc/msm_vdec.h
index 419c8c1..73a516e 100644
--- a/drivers/media/video/msm_vidc/msm_vdec.h
+++ b/drivers/media/video/msm_vidc/msm_vdec.h
@@ -18,6 +18,7 @@
 
 int msm_vdec_inst_init(struct msm_vidc_inst *inst);
 int msm_vdec_ctrl_init(struct msm_vidc_inst *inst);
+int msm_vdec_ctrl_deinit(struct msm_vidc_inst *inst);
 int msm_vdec_querycap(void *instance, struct v4l2_capability *cap);
 int msm_vdec_enum_fmt(void *instance, struct v4l2_fmtdesc *f);
 int msm_vdec_s_fmt(void *instance, struct v4l2_format *f);
diff --git a/drivers/media/video/msm_vidc/msm_venc.c b/drivers/media/video/msm_vidc/msm_venc.c
index 41518d7..0fa56b7 100644
--- a/drivers/media/video/msm_vidc/msm_venc.c
+++ b/drivers/media/video/msm_vidc/msm_venc.c
@@ -88,7 +88,20 @@
 	"High Latency",
 };
 
-static const struct msm_vidc_ctrl msm_venc_ctrls[] = {
+enum msm_venc_ctrl_cluster {
+	MSM_VENC_CTRL_CLUSTER_QP = 1,
+	MSM_VENC_CTRL_CLUSTER_INTRA_PERIOD,
+	MSM_VENC_CTRL_CLUSTER_H264_PROFILE_LEVEL,
+	MSM_VENC_CTRL_CLUSTER_MPEG_PROFILE_LEVEL,
+	MSM_VENC_CTRL_CLUSTER_H263_PROFILE_LEVEL,
+	MSM_VENC_CTRL_CLUSTER_H264_ENTROPY,
+	MSM_VENC_CTRL_CLUSTER_SLICING,
+	MSM_VENC_CTRL_CLUSTER_INTRA_REFRESH,
+	MSM_VENC_CTRL_CLUSTER_BITRATE,
+	MSM_VENC_CTRL_CLUSTER_MAX,
+};
+
+static struct msm_vidc_ctrl msm_venc_ctrls[] = {
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_FRAME_RATE,
 		.name = "Frame Rate",
@@ -99,12 +112,13 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD,
 		.name = "IDR Period",
 		.type = V4L2_CTRL_TYPE_INTEGER,
-		.minimum = 0,
+		.minimum = 1,
 		.maximum = 10*MAX_FRAME_RATE,
 		.default_value = DEFAULT_FRAME_RATE,
 		.step = 1,
@@ -112,6 +126,18 @@
 		.qmenu = NULL,
 	},
 	{
+		.id = V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
+		.name = "Intra Period",
+		.type = V4L2_CTRL_TYPE_INTEGER,
+		.minimum = 1,
+		.maximum = 10*MAX_FRAME_RATE,
+		.default_value = DEFAULT_FRAME_RATE,
+		.step = 1,
+		.menu_skip_mask = 0,
+		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_PERIOD,
+	},
+	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES,
 		.name = "Intra Period for P frames",
 		.type = V4L2_CTRL_TYPE_INTEGER,
@@ -121,17 +147,19 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_PERIOD,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES,
 		.name = "Intra Period for B frames",
 		.type = V4L2_CTRL_TYPE_INTEGER,
 		.minimum = 0,
-		.maximum = 10*DEFAULT_FRAME_RATE,
+		.maximum = 2,
 		.default_value = 0,
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_PERIOD,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_REQUEST_IFRAME,
@@ -143,10 +171,11 @@
 		.step = 0,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL,
-		.name = "Rate Control",
+		.name = "Video Framerate and Bitrate Control",
 		.type = V4L2_CTRL_TYPE_MENU,
 		.minimum = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_OFF,
 		.maximum = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_CFR,
@@ -160,6 +189,22 @@
 		(1 << V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_CFR)
 		),
 		.qmenu = mpeg_video_rate_control,
+		.cluster = MSM_VENC_CTRL_CLUSTER_BITRATE,
+	},
+	{
+		.id = V4L2_CID_MPEG_VIDEO_BITRATE_MODE,
+		.name = "Bitrate Control",
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_BITRATE_MODE_VBR,
+		.maximum = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+		.default_value = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR,
+		.step = 0,
+		.menu_skip_mask = ~(
+		(1 << V4L2_MPEG_VIDEO_BITRATE_MODE_VBR) |
+		(1 << V4L2_MPEG_VIDEO_BITRATE_MODE_CBR)
+		),
+		.qmenu = mpeg_video_rate_control,
+		.cluster = MSM_VENC_CTRL_CLUSTER_BITRATE,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_BITRATE,
@@ -171,6 +216,7 @@
 		.step = BIT_RATE_STEP,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_BITRATE,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE,
@@ -184,6 +230,7 @@
 		(1 << V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CAVLC) |
 		(1 << V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC)
 		),
+		.cluster = MSM_VENC_CTRL_CLUSTER_H264_ENTROPY,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL,
@@ -199,6 +246,7 @@
 		(1 << V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_2)
 		),
 		.qmenu = h264_video_entropy_cabac_model,
+		.cluster = MSM_VENC_CTRL_CLUSTER_H264_ENTROPY,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
@@ -209,6 +257,7 @@
 		.default_value = V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE,
 		.step = 1,
 		.menu_skip_mask = 0,
+		.cluster = MSM_VENC_CTRL_CLUSTER_MPEG_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
@@ -219,6 +268,7 @@
 		.default_value = V4L2_MPEG_VIDEO_MPEG4_LEVEL_0,
 		.step = 1,
 		.menu_skip_mask = 0,
+		.cluster = MSM_VENC_CTRL_CLUSTER_MPEG_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_PROFILE,
@@ -229,6 +279,7 @@
 		.default_value = V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE,
 		.step = 1,
 		.menu_skip_mask = 0,
+		.cluster = MSM_VENC_CTRL_CLUSTER_H264_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_LEVEL,
@@ -239,6 +290,7 @@
 		.default_value = V4L2_MPEG_VIDEO_H264_LEVEL_1_0,
 		.step = 0,
 		.menu_skip_mask = 0,
+		.cluster = MSM_VENC_CTRL_CLUSTER_H264_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_H263_PROFILE,
@@ -259,6 +311,7 @@
 		(1 << V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_HIGHLATENCY)
 		),
 		.qmenu = h263_profile,
+		.cluster = MSM_VENC_CTRL_CLUSTER_H263_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_H263_LEVEL,
@@ -277,6 +330,7 @@
 		(1 << V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_7_0)
 		),
 		.qmenu = h263_level,
+		.cluster = MSM_VENC_CTRL_CLUSTER_H263_PROFILE_LEVEL,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_ROTATION,
@@ -293,6 +347,7 @@
 		(1 << V4L2_CID_MPEG_VIDC_VIDEO_ROTATION_270)
 		),
 		.qmenu = mpeg_video_rotation,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP,
@@ -304,6 +359,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_QP,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP,
@@ -315,6 +371,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_QP,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP,
@@ -326,6 +383,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_QP,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
@@ -336,6 +394,7 @@
 		.default_value = V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE,
 		.step = 1,
 		.menu_skip_mask = 0,
+		.cluster = MSM_VENC_CTRL_CLUSTER_SLICING,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES,
@@ -347,6 +406,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_SLICING,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB,
@@ -358,6 +418,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_SLICING,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE,
@@ -374,6 +435,7 @@
 		(1 << V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_CYCLIC_ADAPTIVE) |
 		(1 << V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_RANDOM)
 		),
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_REFRESH,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS,
@@ -385,6 +447,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_REFRESH,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF,
@@ -396,6 +459,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_REFRESH,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS,
@@ -407,6 +471,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = MSM_VENC_CTRL_CLUSTER_INTRA_REFRESH,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA,
@@ -418,6 +483,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA,
@@ -429,6 +495,7 @@
 		.step = 1,
 		.menu_skip_mask = 0,
 		.qmenu = NULL,
+		.cluster = 0,
 	},
 	{
 		.id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
@@ -443,18 +510,24 @@
 		(1 << V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED) |
 		(1 << L_MODE)
 		),
+		.cluster = 0,
 	},
 	{
-		.id = V4L2_CID_QCOM_VIDEO_SYNC_FRAME_SEQ_HDR,
-		.name = "CodecConfig with sync frame",
-		.type = V4L2_CTRL_TYPE_BOOLEAN,
-		.minimum = 0,
-		.maximum = 1,
-		.default_value = 1,
+		.id = V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+		.name = "Sequence Header Mode",
+		.type = V4L2_CTRL_TYPE_MENU,
+		.minimum = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
+		.maximum = V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_I_FRAME,
+		.default_value =
+			V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_I_FRAME,
 		.step = 1,
-		.menu_skip_mask = 0,
+		.menu_skip_mask = ~(
+		(1 << V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE) |
+		(1 << V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_I_FRAME)
+		),
 		.qmenu = NULL,
-	},
+		.cluster = 0,
+	}
 };
 
 #define NUM_CTRLS ARRAY_SIZE(msm_venc_ctrls)
@@ -476,29 +549,6 @@
 	return sz;
 }
 
-static struct hal_quantization
-	venc_quantization = {I_FRAME_QP, P_FRAME_QP, B_FRAME_QP};
-static struct hal_intra_period
-	venc_intra_period = {2*DEFAULT_FRAME_RATE-1 , 0};
-static struct hal_profile_level
-	venc_h264_profile_level = {HAL_H264_PROFILE_BASELINE,
-		HAL_H264_LEVEL_1};
-static struct hal_profile_level
-	venc_mpeg4_profile_level = {HAL_MPEG4_PROFILE_SIMPLE,
-		HAL_MPEG4_LEVEL_0};
-static struct hal_profile_level
-	venc_h263_profile_level = {HAL_H263_PROFILE_BASELINE,
-				HAL_H263_LEVEL_10};
-static struct hal_h264_entropy_control
-	venc_h264_entropy_control = {HAL_H264_ENTROPY_CAVLC,
-		HAL_H264_CABAC_MODEL_0};
-static struct hal_multi_slice_control
-	venc_multi_slice_control = {HAL_MULTI_SLICE_OFF ,
-		0};
-static struct hal_intra_refresh
-	venc_intra_refresh = {HAL_INTRA_REFRESH_NONE ,
-		DEFAULT_IR_MBS, DEFAULT_IR_MBS, DEFAULT_IR_MBS};
-
 static const struct msm_vidc_format venc_formats[] = {
 	{
 		.name = "YCbCr Semiplanar 4:2:0",
@@ -744,11 +794,162 @@
 	return &msm_venc_vb2q_ops;
 }
 
-static int msm_venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
+static struct v4l2_ctrl *get_ctrl_from_cluster(int id,
+		struct v4l2_ctrl **cluster, int ncontrols)
 {
+	int c;
 
+	for (c = 0; c < ncontrols; ++c)
+		if (cluster[c]->id == id)
+			return cluster[c];
+	return NULL;
+}
+
+/* Helper function to translate V4L2_* to HAL_* */
+static inline int venc_v4l2_to_hal(int id, int value)
+{
+	switch (id) {
+	/* MPEG4 */
+	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0:
+			return HAL_MPEG4_LEVEL_0;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0B:
+			return HAL_MPEG4_LEVEL_0b;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_1:
+			return HAL_MPEG4_LEVEL_1;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_2:
+			return HAL_MPEG4_LEVEL_2;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_3:
+			return HAL_MPEG4_LEVEL_3;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_4:
+			return HAL_MPEG4_LEVEL_4;
+		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_5:
+			return HAL_MPEG4_LEVEL_5;
+		default:
+			goto unknown_value;
+		}
+	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE:
+			return HAL_MPEG4_PROFILE_SIMPLE;
+		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE:
+			return HAL_MPEG4_PROFILE_ADVANCEDSIMPLE;
+		default:
+			goto unknown_value;
+		}
+	/* H264 */
+	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
+			return HAL_H264_PROFILE_BASELINE;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
+			return HAL_H264_PROFILE_MAIN;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED:
+			return HAL_H264_PROFILE_EXTENDED;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
+			return HAL_H264_PROFILE_HIGH;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_10:
+			return HAL_H264_PROFILE_HIGH10;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_422:
+			return HAL_H264_PROFILE_HIGH422;
+		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_444_PREDICTIVE:
+			return HAL_H264_PROFILE_HIGH444;
+		default:
+			goto unknown_value;
+		}
+	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		switch (value) {
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
+			return HAL_H264_LEVEL_1;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
+			return HAL_H264_LEVEL_1b;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
+			return HAL_H264_LEVEL_11;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
+			return HAL_H264_LEVEL_12;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
+			return HAL_H264_LEVEL_13;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
+			return HAL_H264_LEVEL_2;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
+			return HAL_H264_LEVEL_21;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
+			return HAL_H264_LEVEL_22;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
+			return HAL_H264_LEVEL_3;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
+			return HAL_H264_LEVEL_31;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
+			return HAL_H264_LEVEL_32;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
+			return HAL_H264_LEVEL_4;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_1:
+			return HAL_H264_LEVEL_41;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_4_2:
+			return HAL_H264_LEVEL_42;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_5_0:
+			return HAL_H264_LEVEL_3;
+		case V4L2_MPEG_VIDEO_H264_LEVEL_5_1:
+			return HAL_H264_LEVEL_51;
+		default:
+			goto unknown_value;
+		}
+		/* H263 */
+	case V4L2_CID_MPEG_VIDC_VIDEO_H263_PROFILE:
+		switch (value) {
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_BASELINE:
+			return HAL_H263_PROFILE_BASELINE;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_H320CODING:
+			return HAL_H263_PROFILE_H320CODING;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_BACKWARDCOMPATIBLE:
+			return HAL_H263_PROFILE_BACKWARDCOMPATIBLE;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_ISWV2:
+			return HAL_H263_PROFILE_ISWV2;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_ISWV3:
+			return HAL_H263_PROFILE_ISWV3;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_HIGHCOMPRESSION:
+			return HAL_H263_PROFILE_HIGHCOMPRESSION;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_INTERNET:
+			return HAL_H263_PROFILE_INTERNET;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_INTERLACE:
+			return HAL_H263_PROFILE_INTERLACE;
+		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_HIGHLATENCY:
+			return HAL_H263_PROFILE_HIGHLATENCY;
+		default:
+			goto unknown_value;
+		}
+	case V4L2_CID_MPEG_VIDC_VIDEO_H263_LEVEL:
+		switch (value) {
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_1_0:
+			return HAL_H263_LEVEL_10;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_2_0:
+			return HAL_H263_LEVEL_20;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_3_0:
+			return HAL_H263_LEVEL_30;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_4_0:
+			return HAL_H263_LEVEL_40;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_4_5:
+			return HAL_H263_LEVEL_45;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_5_0:
+			return HAL_H263_LEVEL_50;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_6_0:
+			return HAL_H263_LEVEL_60;
+		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_7_0:
+			return HAL_H263_LEVEL_70;
+		default:
+			goto unknown_value;
+		}
+	}
+
+unknown_value:
+	dprintk(VIDC_WARN, "Unknown control (%x, %d)", id, value);
+	return -EINVAL;
+}
+
+static int try_set_ctrl(struct msm_vidc_inst *inst, struct v4l2_ctrl *ctrl)
+{
 	int rc = 0;
-	struct v4l2_control control;
 	struct hal_frame_rate frame_rate;
 	struct hal_request_iframe request_iframe;
 	struct hal_bitrate bitrate;
@@ -762,48 +963,78 @@
 	struct hal_multi_slice_control multi_slice_control;
 	struct hal_h264_db_control h264_db_control;
 	struct hal_enable enable;
-	u32 property_id = 0;
-	u32 property_val = 0;
+	u32 property_id = 0, property_val = 0;
 	void *pdata;
-	struct msm_vidc_inst *inst = container_of(ctrl->handler,
-					struct msm_vidc_inst, ctrl_handler);
-	rc = msm_comm_try_state(inst, MSM_VIDC_OPEN_DONE);
-	if (rc) {
-		dprintk(VIDC_ERR,
-			"Failed to move inst: %p to start done state\n", inst);
-		goto failed_open_done;
-	}
-	control.id = ctrl->id;
-	control.value = ctrl->val;
+	struct v4l2_ctrl *temp_ctrl = NULL;
 
-	switch (control.id) {
+	/* Small helper macro for quickly getting a control and err checking */
+#define TRY_GET_CTRL(__ctrl_id) ({ \
+		struct v4l2_ctrl *__temp; \
+		__temp = get_ctrl_from_cluster( \
+			__ctrl_id, \
+			ctrl->cluster, ctrl->ncontrols); \
+		if (!__temp) { \
+			dprintk(VIDC_ERR, "Can't find %s (%x) in cluster", \
+				#__ctrl_id, __ctrl_id); \
+			rc = -ENOENT; \
+			break; \
+		} \
+		__temp; \
+	})
+
+	switch (ctrl->id) {
 	case V4L2_CID_MPEG_VIDC_VIDEO_FRAME_RATE:
-			property_id =
-				HAL_CONFIG_FRAME_RATE;
-			frame_rate.frame_rate = control.value;
-			frame_rate.buffer_type = HAL_BUFFER_OUTPUT;
-			pdata = &frame_rate;
+		property_id =
+			HAL_CONFIG_FRAME_RATE;
+		frame_rate.frame_rate = ctrl->val;
+		frame_rate.buffer_type = HAL_BUFFER_OUTPUT;
+		pdata = &frame_rate;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD:
 		property_id =
 			HAL_CONFIG_VENC_IDR_PERIOD;
-		idr_period.idr_period = control.value;
-			pdata = &idr_period;
+		idr_period.idr_period = ctrl->val;
+		pdata = &idr_period;
 		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD: {
+		struct v4l2_ctrl *b;
+		b = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES);
+
+		if (inst->fmts[CAPTURE_PORT]->fourcc != V4L2_PIX_FMT_H264 &&
+			inst->fmts[CAPTURE_PORT]->fourcc !=
+				V4L2_PIX_FMT_H264_NO_SC) {
+			dprintk(VIDC_ERR, "Control 0x%x only valid for H264",
+					ctrl->id);
+			rc = -ENOTSUPP;
+			break;
+		}
+
+		/*
+		 * We can't set the I-period explicitly. So set it implicitly
+		 * by setting the number of P and B frames per I-period
+		 */
+		property_id = HAL_CONFIG_VENC_INTRA_PERIOD;
+		intra_period.pframes = (ctrl->val - 1) - b->val;
+		intra_period.bframes = b->val;
+		pdata = &intra_period;
+		break;
+	}
 	case V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES);
+
 		property_id =
 			HAL_CONFIG_VENC_INTRA_PERIOD;
-		intra_period.pframes = control.value;
-		venc_intra_period.pframes = control.value;
-		intra_period.bframes = venc_intra_period.bframes;
+		intra_period.pframes = ctrl->val;
+		intra_period.bframes = temp_ctrl->val;
 		pdata = &intra_period;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES);
+
 		property_id =
 			HAL_CONFIG_VENC_INTRA_PERIOD;
-		intra_period.bframes = control.value;
-		venc_intra_period.bframes = control.value;
-		intra_period.pframes = venc_intra_period.pframes;
+		intra_period.bframes = ctrl->val;
+		intra_period.pframes = temp_ctrl->val;
 		pdata = &intra_period;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_REQUEST_IFRAME:
@@ -812,396 +1043,405 @@
 		request_iframe.enable = true;
 		pdata = &request_iframe;
 		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE_MODE:
+	{
+		bool cfr = true, cbr = true;
+		int final_mode = 0;
+
+		temp_ctrl = TRY_GET_CTRL(
+			V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL);
+
+		switch (temp_ctrl->val) {
+		case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_OFF:
+			/* Let's assume CFR */
+		case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR:
+		case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_CFR:
+			cfr = true;
+			break;
+		case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_VFR:
+		case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_VFR:
+			cfr = false;
+			break;
+		default:
+			dprintk(VIDC_WARN, "Unknown framerate mode");
+		}
+
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_BITRATE_MODE_VBR:
+			cbr = false;
+			break;
+		case V4L2_MPEG_VIDEO_BITRATE_MODE_CBR:
+			cbr = true;
+			break;
+		default:
+			dprintk(VIDC_WARN, "Unknown bitrate mode");
+		}
+
+		if (!cfr && !cbr)
+			final_mode =
+				V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_VFR;
+		else if (!cfr && cbr)
+			final_mode =
+				V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_VFR;
+		else if (cfr && !cbr)
+			final_mode =
+				V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR;
+		else /* ... if (cfr && cbr) */
+			final_mode =
+				V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_CBR_CFR;
+
+		property_id = HAL_PARAM_VENC_RATE_CONTROL;
+		property_val = final_mode;
+		pdata = &property_val;
+		break;
+	}
 	case V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL:
-		property_id =
-			HAL_PARAM_VENC_RATE_CONTROL;
-		property_val = control.value;
+		property_id = HAL_PARAM_VENC_RATE_CONTROL;
+		property_val = ctrl->val;
 		pdata = &property_val;
 		break;
 	case V4L2_CID_MPEG_VIDEO_BITRATE:
 		property_id =
 			HAL_CONFIG_VENC_TARGET_BITRATE;
-		bitrate.bit_rate = control.value;
+		bitrate.bit_rate = ctrl->val;
 		pdata = &bitrate;
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE:
+		temp_ctrl = TRY_GET_CTRL(
+			V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL);
+
 		property_id =
 			HAL_PARAM_VENC_H264_ENTROPY_CONTROL;
-		h264_entropy_control.entropy_mode = control.value;
-		venc_h264_entropy_control.entropy_mode = control.value;
-		h264_entropy_control.cabac_model =
-			venc_h264_entropy_control.cabac_model;
+		h264_entropy_control.entropy_mode = ctrl->val;
+		h264_entropy_control.cabac_model = temp_ctrl->val;
 		pdata = &h264_entropy_control;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE);
+
 		property_id =
 			HAL_PARAM_VENC_H264_ENTROPY_CONTROL;
-		h264_entropy_control.cabac_model = control.value;
-		venc_h264_entropy_control.cabac_model = control.value;
-		h264_entropy_control.entropy_mode =
-			venc_h264_entropy_control.entropy_mode;
+		h264_entropy_control.cabac_model = ctrl->val;
+		h264_entropy_control.entropy_mode = temp_ctrl->val;
 		pdata = &h264_entropy_control;
 		break;
 	case V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-		switch (control.value) {
-		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_SIMPLE:
-			control.value = HAL_MPEG4_PROFILE_SIMPLE;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_PROFILE_ADVANCED_SIMPLE:
-			control.value = HAL_MPEG4_PROFILE_ADVANCEDSIMPLE;
-			break;
-		default:
-			break;
-			}
-		profile_level.profile = control.value;
-		venc_mpeg4_profile_level.profile = control.value;
-		profile_level.level = venc_mpeg4_profile_level.level;
+		profile_level.profile =  venc_v4l2_to_hal(ctrl->id,
+						ctrl->val);
+		profile_level.level = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL,
+				temp_ctrl->val);
 		pdata = &profile_level;
 		break;
 	case V4L2_CID_MPEG_VIDEO_MPEG4_LEVEL:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-		switch (control.value) {
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0:
-			control.value = HAL_MPEG4_LEVEL_0;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_0B:
-			control.value = HAL_MPEG4_LEVEL_0b;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_1:
-			control.value = HAL_MPEG4_LEVEL_1;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_2:
-			control.value = HAL_MPEG4_LEVEL_2;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_3:
-			control.value = HAL_MPEG4_LEVEL_3;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_4:
-			control.value = HAL_MPEG4_LEVEL_4;
-			break;
-		case V4L2_MPEG_VIDEO_MPEG4_LEVEL_5:
-			control.value = HAL_MPEG4_LEVEL_5;
-			break;
-		default:
-			break;
-		}
-		profile_level.level = control.value;
-		venc_mpeg4_profile_level.level = control.value;
-		profile_level.profile = venc_mpeg4_profile_level.profile;
+		profile_level.level = venc_v4l2_to_hal(ctrl->id,
+							ctrl->val);
+		profile_level.profile = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDEO_MPEG4_PROFILE,
+				temp_ctrl->val);
 		pdata = &profile_level;
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_LEVEL);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-
-		switch (control.value) {
-		case V4L2_MPEG_VIDEO_H264_PROFILE_BASELINE:
-			control.value = HAL_H264_PROFILE_BASELINE;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_MAIN:
-			control.value = HAL_H264_PROFILE_MAIN;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_EXTENDED:
-			control.value = HAL_H264_PROFILE_EXTENDED;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH:
-			control.value = HAL_H264_PROFILE_HIGH;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_10:
-			control.value = HAL_H264_PROFILE_HIGH10;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_422:
-			control.value = HAL_H264_PROFILE_HIGH422;
-			break;
-		case V4L2_MPEG_VIDEO_H264_PROFILE_HIGH_444_PREDICTIVE:
-			control.value = HAL_H264_PROFILE_HIGH444;
-			break;
-		default:
-			break;
-			}
-		profile_level.profile = control.value;
-		venc_h264_profile_level.profile = control.value;
-		profile_level.level = venc_h264_profile_level.level;
+		profile_level.profile = venc_v4l2_to_hal(ctrl->id,
+							ctrl->val);
+		profile_level.level = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDEO_H264_LEVEL,
+				temp_ctrl->val);
 		pdata = &profile_level;
 		dprintk(VIDC_DBG, "\nprofile: %d\n",
 			   profile_level.profile);
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_PROFILE);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-
-		switch (control.value) {
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_0:
-			control.value = HAL_H264_LEVEL_1;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1B:
-			control.value = HAL_H264_LEVEL_1b;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_1:
-			control.value = HAL_H264_LEVEL_11;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_2:
-			control.value = HAL_H264_LEVEL_12;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_1_3:
-			control.value = HAL_H264_LEVEL_13;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_0:
-			control.value = HAL_H264_LEVEL_2;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_1:
-			control.value = HAL_H264_LEVEL_21;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_2_2:
-			control.value = HAL_H264_LEVEL_22;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_0:
-			control.value = HAL_H264_LEVEL_3;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_1:
-			control.value = HAL_H264_LEVEL_31;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_3_2:
-			control.value = HAL_H264_LEVEL_32;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_4_0:
-			control.value = HAL_H264_LEVEL_4;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_4_1:
-			control.value = HAL_H264_LEVEL_41;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_4_2:
-			control.value = HAL_H264_LEVEL_42;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_5_0:
-			control.value = HAL_H264_LEVEL_3;
-			break;
-		case V4L2_MPEG_VIDEO_H264_LEVEL_5_1:
-			control.value = HAL_H264_LEVEL_51;
-			break;
-		default:
-			break;
-		}
-		profile_level.level = control.value;
-		venc_h264_profile_level.level = control.value;
-		profile_level.profile = venc_h264_profile_level.profile;
-		pdata = &profile_level;
+		profile_level.level = venc_v4l2_to_hal(ctrl->id,
+							ctrl->val);
+		profile_level.profile = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDEO_H264_PROFILE,
+				temp_ctrl->val);
 		pdata = &profile_level;
 		dprintk(VIDC_DBG, "\nLevel: %d\n",
 			   profile_level.level);
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_H263_PROFILE:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_H263_LEVEL);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-
-		switch (control.value) {
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_BASELINE:
-			control.value = HAL_H263_PROFILE_BASELINE;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_H320CODING:
-			control.value = HAL_H263_PROFILE_H320CODING;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_BACKWARDCOMPATIBLE:
-			control.value = HAL_H263_PROFILE_BACKWARDCOMPATIBLE;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_ISWV2:
-			control.value = HAL_H263_PROFILE_ISWV2;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_ISWV3:
-			control.value = HAL_H263_PROFILE_ISWV3;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_HIGHCOMPRESSION:
-			control.value = HAL_H263_PROFILE_HIGHCOMPRESSION;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_INTERNET:
-			control.value = HAL_H263_PROFILE_INTERNET;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_INTERLACE:
-			control.value = HAL_H263_PROFILE_INTERLACE;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_PROFILE_HIGHLATENCY:
-			control.value = HAL_H263_PROFILE_HIGHLATENCY;
-			break;
-		default:
-			break;
-		}
-		profile_level.profile = control.value;
-		venc_h263_profile_level.profile = control.value;
-		profile_level.level = venc_h263_profile_level.level;
+		profile_level.profile = venc_v4l2_to_hal(ctrl->id,
+							ctrl->val);
+		profile_level.level = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDC_VIDEO_H263_LEVEL,
+				temp_ctrl->val);
 		pdata = &profile_level;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_H263_LEVEL:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_H263_PROFILE);
+
 		property_id =
 			HAL_PARAM_PROFILE_LEVEL_CURRENT;
-
-		switch (control.value) {
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_1_0:
-			control.value = HAL_H263_LEVEL_10;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_2_0:
-			control.value = HAL_H263_LEVEL_20;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_3_0:
-			control.value = HAL_H263_LEVEL_30;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_4_0:
-			control.value = HAL_H263_LEVEL_40;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_4_5:
-			control.value = HAL_H263_LEVEL_45;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_5_0:
-			control.value = HAL_H263_LEVEL_50;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_6_0:
-			control.value = HAL_H263_LEVEL_60;
-			break;
-		case V4L2_MPEG_VIDC_VIDEO_H263_LEVEL_7_0:
-			control.value = HAL_H263_LEVEL_70;
-			break;
-		default:
-			break;
-		}
-
-		profile_level.level = control.value;
-		venc_h263_profile_level.level = control.value;
-		profile_level.profile = venc_h263_profile_level.profile;
+		profile_level.level = venc_v4l2_to_hal(ctrl->id,
+							ctrl->val);
+		profile_level.profile = venc_v4l2_to_hal(
+				V4L2_CID_MPEG_VIDC_VIDEO_H263_PROFILE,
+				ctrl->val);
 		pdata = &profile_level;
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_ROTATION:
 		property_id =
 			HAL_CONFIG_VPE_OPERATIONS;
-		operations.rotate = control.value;
+		operations.rotate = ctrl->val;
 		pdata = &operations;
 		break;
-	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP: {
+		struct v4l2_ctrl *qpp, *qpb;
+
+		qpp = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP);
+		qpb = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP);
+
 		property_id =
 			HAL_PARAM_VENC_SESSION_QP;
-		quantization.qpi = control.value;
-		venc_quantization.qpi = control.value;
-		quantization.qpp = venc_quantization.qpp;
-		quantization.qpb = venc_quantization.qpb;
+		quantization.qpi = ctrl->val;
+		quantization.qpp = qpp->val;
+		quantization.qpb = qpb->val;
+
 		pdata = &quantization;
 		break;
-	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+	}
+	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP: {
+		struct v4l2_ctrl *qpi, *qpb;
+
+		qpi = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP);
+		qpb = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP);
+
 		property_id =
 			HAL_PARAM_VENC_SESSION_QP;
-		quantization.qpp = control.value;
-		venc_quantization.qpp = control.value;
-		quantization.qpi = venc_quantization.qpi;
-		quantization.qpb = venc_quantization.qpb;
+		quantization.qpp = ctrl->val;
+		quantization.qpi = qpi->val;
+		quantization.qpb = qpb->val;
+
 		pdata = &quantization;
 		break;
-	case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP:
+	}
+	case V4L2_CID_MPEG_VIDEO_H264_B_FRAME_QP: {
+		struct v4l2_ctrl *qpi, *qpp;
+
+		qpi = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP);
+		qpp = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP);
+
 		property_id =
 			HAL_PARAM_VENC_SESSION_QP;
-		quantization.qpb = control.value;
-		venc_quantization.qpb = control.value;
-		quantization.qpi = venc_quantization.qpi;
-		quantization.qpp = venc_quantization.qpp;
+		quantization.qpb = ctrl->val;
+		quantization.qpi = qpi->val;
+		quantization.qpp = qpp->val;
+
 		pdata = &quantization;
 		break;
-	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+	}
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE: {
+		int temp = 0;
+
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB:
+			temp = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB;
+			break;
+		case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES:
+			temp = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES;
+			break;
+		case V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE:
+		default:
+			temp = 0;
+			break;
+		}
+
+		if (temp)
+			temp_ctrl = TRY_GET_CTRL(temp);
+
 		property_id =
 			HAL_PARAM_VENC_MULTI_SLICE_CONTROL;
-		multi_slice_control.multi_slice = control.value;
-		venc_multi_slice_control.multi_slice = control.value;
-		multi_slice_control.slice_size =
-			venc_multi_slice_control.slice_size;
+		multi_slice_control.multi_slice = ctrl->val;
+		multi_slice_control.slice_size = temp ? temp_ctrl->val : 0;
+
 		pdata = &multi_slice_control;
 		break;
+	}
 	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
 	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+		temp_ctrl = TRY_GET_CTRL(V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE);
+
 		property_id =
 			HAL_PARAM_VENC_MULTI_SLICE_CONTROL;
-		multi_slice_control.multi_slice =
-			venc_multi_slice_control.multi_slice;
-		multi_slice_control.slice_size = control.value;
-		venc_multi_slice_control.slice_size = control.value;
+		multi_slice_control.multi_slice = temp_ctrl->val;
+		multi_slice_control.slice_size = ctrl->val;
 		pdata = &multi_slice_control;
 		break;
-	case V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE:
+	case V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE: {
+		struct v4l2_ctrl *air_mbs, *air_ref, *cir_mbs;
+		air_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS);
+		air_ref = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF);
+		cir_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS);
+
 		property_id =
 			HAL_PARAM_VENC_INTRA_REFRESH;
-		intra_refresh.mode = control.value;
-		intra_refresh.air_mbs = venc_intra_refresh.air_mbs;
-		intra_refresh.air_ref = venc_intra_refresh.air_ref;
-		intra_refresh.cir_mbs = venc_intra_refresh.cir_mbs;
-		venc_intra_refresh.mode = intra_refresh.mode;
+
+		intra_refresh.mode = ctrl->val;
+		intra_refresh.air_mbs = air_mbs->val;
+		intra_refresh.air_ref = air_ref->val;
+		intra_refresh.cir_mbs = cir_mbs->val;
+
 		pdata = &intra_refresh;
 		break;
-	case V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS:
+	}
+	case V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS: {
+		struct v4l2_ctrl *ir_mode, *air_ref, *cir_mbs;
+		ir_mode = TRY_GET_CTRL(
+				V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE);
+		air_ref = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF);
+		cir_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS);
+
 		property_id =
 			HAL_PARAM_VENC_INTRA_REFRESH;
-		intra_refresh.air_mbs = control.value;
-		intra_refresh.mode = venc_intra_refresh.mode;
-		intra_refresh.air_ref = venc_intra_refresh.air_ref;
-		intra_refresh.cir_mbs = venc_intra_refresh.cir_mbs;
-		venc_intra_refresh.air_mbs = control.value;
+		intra_refresh.air_mbs = ctrl->val;
+		intra_refresh.mode = ir_mode->val;
+		intra_refresh.air_ref = air_ref->val;
+		intra_refresh.cir_mbs = cir_mbs->val;
+
 		pdata = &intra_refresh;
 		break;
-	case V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF:
+	}
+	case V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF: {
+		struct v4l2_ctrl *ir_mode, *air_mbs, *cir_mbs;
+		ir_mode = TRY_GET_CTRL(
+				V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE);
+		air_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS);
+		cir_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS);
+
 		property_id =
 			HAL_PARAM_VENC_INTRA_REFRESH;
-		intra_refresh.air_ref = control.value;
-		intra_refresh.air_mbs = venc_intra_refresh.air_mbs;
-		intra_refresh.mode = venc_intra_refresh.mode;
-		intra_refresh.cir_mbs = venc_intra_refresh.cir_mbs;
-		venc_intra_refresh.air_ref = control.value;
+		intra_refresh.air_ref = ctrl->val;
+		intra_refresh.air_mbs = air_mbs->val;
+		intra_refresh.mode = ir_mode->val;
+		intra_refresh.cir_mbs = cir_mbs->val;
+
 		pdata = &intra_refresh;
 		break;
-	case V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS:
+	}
+	case V4L2_CID_MPEG_VIDC_VIDEO_CIR_MBS: {
+		struct v4l2_ctrl *ir_mode, *air_mbs, *air_ref;
+
+		ir_mode = TRY_GET_CTRL(
+				V4L2_CID_MPEG_VIDC_VIDEO_INTRA_REFRESH_MODE);
+		air_mbs = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_MBS);
+		air_ref = TRY_GET_CTRL(V4L2_CID_MPEG_VIDC_VIDEO_AIR_REF);
+
 		property_id =
 			HAL_PARAM_VENC_INTRA_REFRESH;
-		intra_refresh.cir_mbs = control.value;
-		intra_refresh.air_mbs = venc_intra_refresh.air_mbs;
-		intra_refresh.air_ref = venc_intra_refresh.air_ref;
-		intra_refresh.mode = venc_intra_refresh.mode;
-		venc_intra_refresh.cir_mbs = control.value;
+
+		intra_refresh.cir_mbs = ctrl->val;
+		intra_refresh.air_mbs = air_mbs->val;
+		intra_refresh.air_ref = air_ref->val;
+		intra_refresh.mode = ir_mode->val;
+
 		pdata = &intra_refresh;
 		break;
+	}
 	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
 		property_id =
 			HAL_PARAM_VENC_H264_DEBLOCK_CONTROL;
-		h264_db_control.mode = control.value;
+		h264_db_control.mode = ctrl->val;
 		pdata = &h264_db_control;
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
 		property_id =
 			HAL_PARAM_VENC_H264_DEBLOCK_CONTROL;
-		h264_db_control.slice_alpha_offset = control.value;
+		h264_db_control.slice_alpha_offset = ctrl->val;
 		pdata = &h264_db_control;
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
 		property_id =
 			HAL_PARAM_VENC_H264_DEBLOCK_CONTROL;
-		h264_db_control.slice_beta_offset = control.value;
+		h264_db_control.slice_beta_offset = ctrl->val;
 		pdata = &h264_db_control;
 		break;
-	case V4L2_CID_QCOM_VIDEO_SYNC_FRAME_SEQ_HDR:
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
 		property_id =
 			HAL_PARAM_VENC_SYNC_FRAME_SEQUENCE_HEADER;
-		enable.enable = control.value;
+
+		switch (ctrl->val) {
+		case V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE:
+			enable.enable = 0;
+			break;
+		case V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_I_FRAME:
+			enable.enable = 1;
+			break;
+		case V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME:
+		default:
+			rc = -ENOTSUPP;
+			break;
+		}
 		pdata = &enable;
 		break;
 	default:
+		rc = -ENOTSUPP;
 		break;
 	}
+#undef TRY_GET_CTRL
+
 	if (property_id) {
 		dprintk(VIDC_DBG, "Control: HAL property=%d,ctrl_value=%d\n",
 				property_id,
-				control.value);
+				ctrl->val);
 		rc = vidc_hal_session_set_property((void *)inst->session,
 				property_id, pdata);
 	}
-	if (rc)
-		dprintk(VIDC_ERR, "Failed to set hal property for framesize\n");
-failed_open_done:
+
 	return rc;
 }
+
+static int msm_venc_op_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+
+	int rc = 0, c = 0;
+	struct msm_vidc_inst *inst = container_of(ctrl->handler,
+					struct msm_vidc_inst, ctrl_handler);
+	rc = msm_comm_try_state(inst, MSM_VIDC_OPEN_DONE);
+
+	if (rc) {
+		dprintk(VIDC_ERR,
+			"Failed to move inst: %p to start done state\n", inst);
+		goto failed_open_done;
+	}
+
+	for (c = 0; c < ctrl->ncontrols; ++c) {
+		if (ctrl->cluster[c]->is_new) {
+			rc = try_set_ctrl(inst, ctrl->cluster[c]);
+			if (rc) {
+				dprintk(VIDC_ERR, "Failed setting %x",
+						ctrl->cluster[c]->id);
+				break;
+			}
+		}
+	}
+failed_open_done:
+	if (rc)
+		dprintk(VIDC_ERR, "Failed to set hal property\n");
+	return rc;
+}
+
 static int msm_venc_op_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 {
 	return 0;
@@ -1334,12 +1574,10 @@
 	if (a->parm.output.timeperframe.denominator) {
 		switch (a->type) {
 		case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
-			us_per_frame = a->parm.output.timeperframe.numerator/
-				a->parm.output.timeperframe.denominator;
-			break;
 		case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
-			us_per_frame = a->parm.capture.timeperframe.numerator/
-				a->parm.capture.timeperframe.denominator;
+			us_per_frame = a->parm.output.timeperframe.numerator *
+				USEC_PER_SEC / a->parm.output.\
+				timeperframe.denominator;
 			break;
 		default:
 			dprintk(VIDC_ERR,
@@ -1665,6 +1903,26 @@
 	return rc;
 }
 
+static struct v4l2_ctrl **get_cluster(int type, int *size)
+{
+	int c = 0, sz = 0;
+	struct v4l2_ctrl **cluster = kmalloc(sizeof(struct v4l2_ctrl *) *
+			NUM_CTRLS, GFP_KERNEL);
+
+	if (type <= 0 || !size || !cluster)
+		return NULL;
+
+	for (c = 0; c < NUM_CTRLS; c++) {
+		if (msm_venc_ctrls[c].cluster == type) {
+			cluster[sz] = msm_venc_ctrls[c].priv;
+			++sz;
+		}
+	}
+
+	*size = sz;
+	return cluster;
+}
+
 int msm_venc_ctrl_init(struct msm_vidc_inst *inst)
 {
 
@@ -1679,6 +1937,7 @@
 	}
 
 	for (; idx < NUM_CTRLS; idx++) {
+		struct v4l2_ctrl *ctrl = NULL;
 		if (IS_PRIV_CTRL(msm_venc_ctrls[idx].id)) {
 			ctrl_cfg.def = msm_venc_ctrls[idx].default_value;
 			ctrl_cfg.flags = 0;
@@ -1692,18 +1951,20 @@
 			ctrl_cfg.step = msm_venc_ctrls[idx].step;
 			ctrl_cfg.type = msm_venc_ctrls[idx].type;
 			ctrl_cfg.qmenu = msm_venc_ctrls[idx].qmenu;
-			v4l2_ctrl_new_custom(&inst->ctrl_handler,
-				&ctrl_cfg, NULL);
+			ctrl = v4l2_ctrl_new_custom(
+					&inst->ctrl_handler,
+					&ctrl_cfg, NULL);
 		} else {
 			if (msm_venc_ctrls[idx].type == V4L2_CTRL_TYPE_MENU) {
-				v4l2_ctrl_new_std_menu(&inst->ctrl_handler,
+				ctrl = v4l2_ctrl_new_std_menu(
+					&inst->ctrl_handler,
 					&msm_venc_ctrl_ops,
 					msm_venc_ctrls[idx].id,
 					msm_venc_ctrls[idx].maximum,
 					msm_venc_ctrls[idx].menu_skip_mask,
 					msm_venc_ctrls[idx].default_value);
 			} else {
-				v4l2_ctrl_new_std(&inst->ctrl_handler,
+				ctrl = v4l2_ctrl_new_std(&inst->ctrl_handler,
 					&msm_venc_ctrl_ops,
 					msm_venc_ctrls[idx].id,
 					msm_venc_ctrls[idx].minimum,
@@ -1712,11 +1973,51 @@
 					msm_venc_ctrls[idx].default_value);
 			}
 		}
+
+		msm_venc_ctrls[idx].priv = ctrl;
 	}
 	ret_val = inst->ctrl_handler.error;
 	if (ret_val)
 		dprintk(VIDC_ERR,
 			"CTRL ERR: Error adding ctrls to ctrl handle, %d\n",
 			inst->ctrl_handler.error);
+
+	/* Construct clusters */
+	for (idx = 1; idx < MSM_VENC_CTRL_CLUSTER_MAX; ++idx) {
+		struct msm_vidc_ctrl_cluster *temp = NULL;
+		struct v4l2_ctrl **cluster = NULL;
+		int cluster_size = 0;
+
+		cluster = get_cluster(idx, &cluster_size);
+		if (!cluster || !cluster_size) {
+			dprintk(VIDC_WARN, "Failed to setup cluster of type %d",
+					idx);
+			continue;
+		}
+
+		v4l2_ctrl_cluster(cluster_size, cluster);
+
+		temp = kzalloc(sizeof(*temp), GFP_KERNEL);
+		if (!temp) {
+			ret_val = -ENOMEM;
+			break;
+		}
+
+		temp->cluster = cluster;
+		INIT_LIST_HEAD(&temp->list);
+		list_add_tail(&temp->list, &inst->ctrl_clusters);
+	}
+
 	return ret_val;
 }
+
+int msm_venc_ctrl_deinit(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_ctrl_cluster *curr, *next;
+	list_for_each_entry_safe(curr, next, &inst->ctrl_clusters, list) {
+		kfree(curr->cluster);
+		kfree(curr);
+	}
+
+	return 0;
+}
diff --git a/drivers/media/video/msm_vidc/msm_venc.h b/drivers/media/video/msm_vidc/msm_venc.h
index ad63e7d..c098773 100644
--- a/drivers/media/video/msm_vidc/msm_venc.h
+++ b/drivers/media/video/msm_vidc/msm_venc.h
@@ -18,6 +18,7 @@
 
 int msm_venc_inst_init(struct msm_vidc_inst *inst);
 int msm_venc_ctrl_init(struct msm_vidc_inst *inst);
+int msm_venc_ctrl_deinit(struct msm_vidc_inst *inst);
 int msm_venc_querycap(void *instance, struct v4l2_capability *cap);
 int msm_venc_enum_fmt(void *instance, struct v4l2_fmtdesc *f);
 int msm_venc_s_fmt(void *instance, struct v4l2_format *f);
diff --git a/drivers/media/video/msm_vidc/msm_vidc.c b/drivers/media/video/msm_vidc/msm_vidc.c
index 64897c7..13180c5 100644
--- a/drivers/media/video/msm_vidc/msm_vidc.c
+++ b/drivers/media/video/msm_vidc/msm_vidc.c
@@ -428,6 +428,7 @@
 	INIT_LIST_HEAD(&inst->pendingq);
 	INIT_LIST_HEAD(&inst->internalbufs);
 	INIT_LIST_HEAD(&inst->persistbufs);
+	INIT_LIST_HEAD(&inst->ctrl_clusters);
 	init_waitqueue_head(&inst->kernel_event_queue);
 	inst->state = MSM_VIDC_CORE_UNINIT_DONE;
 	inst->core = core;
@@ -555,10 +556,18 @@
 			list_del(&inst->list);
 	}
 	mutex_unlock(&core->sync_lock);
+
+	if (inst->session_type == MSM_VIDC_DECODER)
+		msm_vdec_ctrl_deinit(inst);
+	else if (inst->session_type == MSM_VIDC_ENCODER)
+		msm_venc_ctrl_deinit(inst);
+
 	cleanup_instance(inst);
 	if (inst->state != MSM_VIDC_CORE_INVALID &&
 		core->state != VIDC_CORE_INVALID)
 		rc = msm_comm_try_state(inst, MSM_VIDC_CORE_UNINIT);
+	else
+		rc = msm_comm_force_cleanup(inst);
 	if (rc)
 		dprintk(VIDC_ERR,
 			"Failed to move video instance to uninit state\n");
diff --git a/drivers/media/video/msm_vidc/msm_vidc_common.c b/drivers/media/video/msm_vidc/msm_vidc_common.c
index 46a88c2..e00528f 100644
--- a/drivers/media/video/msm_vidc/msm_vidc_common.c
+++ b/drivers/media/video/msm_vidc/msm_vidc_common.c
@@ -24,7 +24,6 @@
 #include "vidc_hal_api.h"
 #include "msm_smem.h"
 #include "msm_vidc_debug.h"
-#include "msm_vidc_ssr.h"
 
 #define HW_RESPONSE_TIMEOUT (5 * 60 * 1000)
 
@@ -63,6 +62,8 @@
 	int ret;
 };
 
+#define TIME_DIFF_THRESHOLD 200
+
 static const u32 bus_table[] = {
 	36000,
 	110400,
@@ -414,7 +415,7 @@
 	complete(&core->completions[SYS_MSG_INDEX(cmd)]);
 }
 
-void change_inst_state(struct msm_vidc_inst *inst,
+static void change_inst_state(struct msm_vidc_inst *inst,
 	enum instance_state state)
 {
 	unsigned long flags;
@@ -683,11 +684,34 @@
 
 static void handle_sys_watchdog_timeout(enum command_response cmd, void *data)
 {
-	dprintk(VIDC_ERR,
-		"msm_vidc: Sub System Restart initiated\n");
+	struct msm_vidc_cb_cmd_done *response = data;
+	struct msm_vidc_inst *inst;
+	struct msm_vidc_core *core = NULL;
+	struct v4l2_event dqevent;
+	unsigned long flags;
+	dprintk(VIDC_ERR, "Venus Subsystem crashed\n");
+	core = get_vidc_core(response->device_id);
+	if (!core) {
+		dprintk(VIDC_ERR, "Wrong device_id received\n");
+		return;
+	}
+	subsystem_crashed("venus");
+	spin_lock_irqsave(&core->lock, flags);
+	core->state = VIDC_CORE_INVALID;
+	spin_unlock_irqrestore(&core->lock, flags);
+	dqevent.type = V4L2_EVENT_MSM_VIDC_SYS_ERROR;
+	dqevent.id = 0;
+	list_for_each_entry(inst, &core->instances, list) {
+		if (inst) {
+			v4l2_event_queue_fh(&inst->event_handler, &dqevent);
+			spin_lock_irqsave(&inst->lock, flags);
+			inst->state = MSM_VIDC_CORE_INVALID;
+			inst->session = NULL;
+			spin_unlock_irqrestore(&inst->lock, flags);
+		}
+	}
 }
 
-
 static void handle_session_close(enum command_response cmd, void *data)
 {
 	struct msm_vidc_cb_cmd_done *response = data;
@@ -729,7 +753,7 @@
 	mutex_unlock(&bufq->lock);
 	if (!found) {
 		dprintk(VIDC_ERR,
-			"Failed to find the buffer in queued list: %d, %d\n",
+			"Failed to find the buffer in queued list: 0x%x, %d\n",
 			dev_addr, q->type);
 		vb = NULL;
 	}
@@ -756,12 +780,63 @@
 	}
 }
 
+static void msm_comm_update_clocks(struct msm_vidc_inst *inst,
+	u64 cur_time_stamp)
+{
+	u32 new_time_diff = 0, cur_time_diff = 0;
+	u8 updated_fps = 0;
+	struct v4l2_ctrl *ctrl = NULL;
+	u32 output_order = 0;
+
+	if (inst->session_type == MSM_VIDC_ENCODER)
+		goto exit;
+	if (cur_time_stamp >= LLONG_MAX) {
+		dprintk(VIDC_WARN,
+			"Clock scaling failed : Timestamp invalid\n");
+		goto exit;
+	}
+	ctrl = v4l2_ctrl_find(&inst->ctrl_handler,
+		V4L2_CID_MPEG_VIDC_VIDEO_OUTPUT_ORDER);
+	if (!ctrl) {
+		dprintk(VIDC_WARN, "Unable to find output order control\n");
+		dprintk(VIDC_WARN,
+			"Performance might be impacted for higher fps clips\n");
+		goto exit;
+	}
+	output_order = v4l2_ctrl_g_ctrl(ctrl);
+	if (output_order == V4L2_MPEG_VIDC_VIDEO_OUTPUT_ORDER_DISPLAY) {
+		new_time_diff =
+			(u32)(cur_time_stamp - inst->prop.prev_time_stamp);
+		inst->prop.prev_time_stamp = cur_time_stamp;
+		if (!new_time_diff)
+			goto exit;
+		if (inst->prop.fps)
+			cur_time_diff = USEC_PER_SEC / inst->prop.fps;
+		cur_time_diff = cur_time_diff > new_time_diff ?
+			cur_time_diff - new_time_diff :
+			new_time_diff - cur_time_diff;
+		if (cur_time_diff > TIME_DIFF_THRESHOLD) {
+			updated_fps = (u8) (USEC_PER_SEC / new_time_diff);
+			if (updated_fps && (updated_fps != inst->prop.fps)) {
+				inst->prop.fps = updated_fps;
+				dprintk(VIDC_DBG,
+						"Updating clocks: Decoding fps = %d\n",
+						inst->prop.fps);
+				msm_comm_scale_clocks_and_bus(inst);
+			}
+		}
+	}
+exit:
+	return;
+}
+
 static void handle_fbd(enum command_response cmd, void *data)
 {
 	struct msm_vidc_cb_data_done *response = data;
 	struct msm_vidc_inst *inst;
 	struct vb2_buffer *vb;
 	struct vidc_hal_fbd *fill_buf_done;
+
 	if (!response) {
 		dprintk(VIDC_ERR, "Invalid response from vidc_hal\n");
 		return;
@@ -777,9 +852,9 @@
 			int64_t time_usec = fill_buf_done->timestamp_hi;
 			time_usec = (time_usec << 32) |
 				fill_buf_done->timestamp_lo;
-
 			vb->v4l2_buf.timestamp =
 				ns_to_timeval(time_usec * NSEC_PER_USEC);
+				msm_comm_update_clocks(inst, time_usec);
 		}
 		vb->v4l2_buf.flags = 0;
 
@@ -789,7 +864,8 @@
 			vb->v4l2_buf.flags &= ~V4L2_QCOM_BUF_FLAG_CODECCONFIG;
 		if (fill_buf_done->flags1 & HAL_BUFFERFLAG_SYNCFRAME)
 			vb->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
-
+		if (fill_buf_done->flags1 & HAL_BUFFERFLAG_EOSEQ)
+			vb->v4l2_buf.flags |= V4L2_QCOM_BUF_FLAG_EOSEQ;
 		switch (fill_buf_done->picture_type) {
 		case HAL_PICTURE_IDR:
 		case HAL_PICTURE_I:
@@ -1013,16 +1089,17 @@
 	}
 	if (msm_comm_scale_clocks(core)) {
 		dprintk(VIDC_WARN,
-		"Failed to scale clocks. Performance might be impacted\n");
+				"Failed to scale clocks. Performance might be impacted\n");
 	}
 	if (msm_comm_scale_bus(core, inst->session_type, DDR_MEM)) {
 		dprintk(VIDC_WARN,
-		"Failed to scale DDR bus. Performance might be impacted\n");
+				"Failed to scale DDR bus. Performance might be impacted\n");
 	}
 	if (core->resources.ocmem.buf) {
-		if (msm_comm_scale_bus(core, inst->session_type, OCMEM_MEM))
+		if (msm_comm_scale_bus(core, inst->session_type,
+					OCMEM_MEM))
 			dprintk(VIDC_WARN,
-			"Failed to scale OCMEM bus. Performance might be impacted\n");
+					"Failed to scale OCMEM bus. Performance might be impacted\n");
 	}
 }
 
@@ -1070,7 +1147,7 @@
 	return rc;
 }
 
-void msm_comm_unload_fw(struct msm_vidc_core *core)
+static void msm_comm_unload_fw(struct msm_vidc_core *core)
 {
 	if (!core) {
 		dprintk(VIDC_ERR, "Invalid paramter: %p\n", core);
@@ -1117,7 +1194,7 @@
 	return rc;
 }
 
-int msm_comm_unset_ocmem(struct msm_vidc_core *core)
+static int msm_comm_unset_ocmem(struct msm_vidc_core *core)
 {
 	struct vidc_resource_hdr rhdr;
 	int rc = 0;
@@ -1125,6 +1202,11 @@
 		dprintk(VIDC_ERR, "Invalid params, core:%p\n",	core);
 		return -EINVAL;
 	}
+	if (core->state == VIDC_CORE_INVALID) {
+		dprintk(VIDC_ERR,
+				"Core is in bad state. Cannot unset ocmem\n");
+		return -EIO;
+	}
 	rhdr.resource_id = VIDC_RESOURCE_OCMEM;
 	rhdr.resource_handle = (u32) &core->resources.ocmem;
 	init_completion(
@@ -1183,7 +1265,7 @@
 	return rc;
 }
 
-int msm_comm_free_ocmem(struct msm_vidc_core *core)
+static int msm_comm_free_ocmem(struct msm_vidc_core *core)
 {
 	int rc = 0;
 	if (core->resources.ocmem.buf) {
@@ -1345,6 +1427,11 @@
 	return rc;
 }
 
+int msm_comm_force_cleanup(struct msm_vidc_inst *inst)
+{
+	return msm_vidc_deinit_core(inst);
+}
+
 static enum hal_domain get_hal_domain(int session_type)
 {
 	enum hal_domain domain;
@@ -1700,7 +1787,6 @@
 				inst->state, state);
 	return rc;
 }
-
 int msm_comm_qbuf(struct vb2_buffer *vb)
 {
 	int rc = 0;
diff --git a/drivers/media/video/msm_vidc/msm_vidc_common.h b/drivers/media/video/msm_vidc/msm_vidc_common.h
index 916a3ca..27eab6f 100644
--- a/drivers/media/video/msm_vidc/msm_vidc_common.h
+++ b/drivers/media/video/msm_vidc/msm_vidc_common.h
@@ -36,11 +36,7 @@
 int msm_comm_flush(struct msm_vidc_inst *inst, u32 flags);
 int msm_comm_release_scratch_buffers(struct msm_vidc_inst *inst);
 int msm_comm_release_persist_buffers(struct msm_vidc_inst *inst);
-void msm_comm_unload_fw(struct msm_vidc_core *core);
-void change_inst_state(struct msm_vidc_inst *inst,
-	enum instance_state state);
-int msm_comm_unset_ocmem(struct msm_vidc_core *core);
-int msm_comm_free_ocmem(struct msm_vidc_core *core);
+int msm_comm_force_cleanup(struct msm_vidc_inst *inst);
 enum hal_extradata_id msm_comm_get_hal_extradata_index(
 	enum v4l2_mpeg_vidc_extradata index);
 #define IS_PRIV_CTRL(idx) (\
diff --git a/drivers/media/video/msm_vidc/msm_vidc_internal.h b/drivers/media/video/msm_vidc/msm_vidc_internal.h
index b274d13..aadb4c2 100644
--- a/drivers/media/video/msm_vidc/msm_vidc_internal.h
+++ b/drivers/media/video/msm_vidc/msm_vidc_internal.h
@@ -170,6 +170,7 @@
 	u32 height;
 	u32 fps;
 	u32 bitrate;
+	u64 prev_time_stamp;
 };
 
 struct buf_queue {
@@ -208,13 +209,6 @@
 	int samples;
 };
 
-struct msm_vidc_ssr_info {
-	struct subsys_device *msm_vidc_dev;
-	struct subsys_desc *msm_vidc_subsys_desc;
-	void *msm_vidc_ramdump_dev;
-	bool ssr_in_progress;
-};
-
 enum msm_vidc_mode {
 	VIDC_NON_SECURE,
 	VIDC_SECURE,
@@ -237,7 +231,6 @@
 	enum vidc_core_state state;
 	struct msm_vidc_resources resources;
 	struct completion completions[SYS_MSG_END - SYS_MSG_START + 1];
-	struct msm_vidc_ssr_info ssr_info;
 };
 
 struct msm_vidc_inst {
@@ -258,6 +251,7 @@
 	void *mem_client;
 	struct v4l2_ctrl_handler ctrl_handler;
 	struct completion completions[SESSION_MSG_END - SESSION_MSG_START + 1];
+	struct list_head ctrl_clusters;
 	struct v4l2_fh event_handler;
 	struct msm_smem *extradata_handle;
 	wait_queue_head_t kernel_event_queue;
@@ -275,6 +269,11 @@
 
 extern struct msm_vidc_drv *vidc_driver;
 
+struct msm_vidc_ctrl_cluster {
+	struct v4l2_ctrl **cluster;
+	struct list_head list;
+};
+
 struct msm_vidc_ctrl {
 	u32 id;
 	char name[MAX_NAME_LENGTH];
@@ -285,6 +284,8 @@
 	u32 step;
 	u32 menu_skip_mask;
 	const char * const *qmenu;
+	u32 cluster;
+	struct v4l2_ctrl *priv;
 };
 
 void handle_cmd_response(enum command_response cmd, void *data);
diff --git a/drivers/media/video/msm_vidc/msm_vidc_ssr.c b/drivers/media/video/msm_vidc/msm_vidc_ssr.c
deleted file mode 100644
index 33464c6f..0000000
--- a/drivers/media/video/msm_vidc/msm_vidc_ssr.c
+++ /dev/null
@@ -1,175 +0,0 @@
-/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-#include <msm_vidc_ssr.h>
-
-static struct msm_vidc_core *get_vidc_core_from_dev(struct device *dev)
-{
-	struct video_device *vdev;
-	struct msm_video_device *videodev;
-	struct msm_vidc_core *core;
-	vdev = container_of(dev, struct video_device, dev);
-	videodev = container_of(vdev, struct msm_video_device, vdev);
-	core = container_of(videodev, struct msm_vidc_core,
-		vdev[MSM_VIDC_DECODER]);
-	return core;
-}
-int msm_vidc_shutdown(const struct subsys_desc *subsys)
-{
-	struct msm_vidc_inst *inst;
-	struct msm_vidc_core *core = NULL;
-	struct v4l2_event dqevent;
-	struct device *dev;
-	unsigned long flags;
-	int rc = 0;
-	if (!subsys) {
-		dprintk(VIDC_ERR, "Invalid subsys: %p\n", subsys);
-		rc = -EINVAL;
-		goto exit;
-	}
-	dev = subsys->dev;
-	if (dev)
-		core = get_vidc_core_from_dev(dev);
-	if (!core) {
-		dprintk(VIDC_ERR, "Invalid core: %p\n", core);
-		rc = -EINVAL;
-		goto exit;
-	}
-	core->ssr_info.ssr_in_progress = true;
-	spin_lock_irqsave(&core->lock, flags);
-	core->state = VIDC_CORE_INVALID;
-	spin_unlock_irqrestore(&core->lock, flags);
-	dqevent.type = V4L2_EVENT_MSM_VIDC_SYS_ERROR;
-	dqevent.id = 0;
-	list_for_each_entry(inst, &core->instances, list) {
-		if (inst) {
-			v4l2_event_queue_fh(&inst->event_handler, &dqevent);
-			spin_lock_irqsave(&inst->lock, flags);
-			inst->state = MSM_VIDC_CORE_INVALID;
-			spin_unlock_irqrestore(&inst->lock, flags);
-		}
-	}
-exit:
-	return rc;
-}
-int msm_vidc_ramdump(int enable, const struct subsys_desc *subsys)
-{
-	struct ramdump_segment memory_segments[] = {{0x0f500000, 0xFF000} };
-	struct msm_vidc_core *core = NULL;
-	void *dump_addr = NULL;
-	int rc = 0;
-	struct device *dev;
-	if (!subsys) {
-		dprintk(VIDC_ERR, "Invalid subsys: %p\n", subsys);
-		rc = -EINVAL;
-		goto exit;
-	}
-	dev = subsys->dev;
-	if (dev)
-		core = get_vidc_core_from_dev(dev);
-	if (!core) {
-		dprintk(VIDC_ERR, "Invalid core: %p\n", core);
-		rc = -EINVAL;
-		goto exit;
-	}
-	if (enable) {
-		rc = do_ramdump(core->ssr_info.msm_vidc_ramdump_dev,
-			memory_segments,
-			ARRAY_SIZE(memory_segments));
-		if (rc < 0)
-			dprintk(VIDC_DBG, "Failed : FW image memory dump\n");
-		dump_addr = kzalloc(core->resources.ocmem.buf->len, GFP_KERNEL);
-		if (dump_addr)
-			rc = ocmem_dump(OCMEM_VIDEO, core->resources.ocmem.buf,
-				(unsigned long)dump_addr);
-		if (rc < 0) {
-			dprintk(VIDC_DBG, "Failed : OCMEM copy\n");
-		} else	{
-			memory_segments[0].address = (unsigned long)dump_addr;
-			memory_segments[0].size =
-				(unsigned long)core->resources.ocmem.buf->len;
-			rc = do_ramdump(core->ssr_info.msm_vidc_ramdump_dev,
-				memory_segments,
-				ARRAY_SIZE(memory_segments));
-			if (rc < 0)
-				dprintk(VIDC_DBG, "Failed : OCMEM dump\n");
-		}
-		kfree(dump_addr);
-	}
-exit:
-	return rc;
-}
-int msm_vidc_powerup(const struct subsys_desc *subsys)
-{
-	unsigned long flags;
-	struct msm_vidc_core *core = NULL;
-	int rc = 0;
-	struct device *dev;
-	if (!subsys) {
-		dprintk(VIDC_ERR, "Invalid subsys: %p\n", subsys);
-		rc = -EINVAL;
-		goto exit;
-	}
-	dev = subsys->dev;
-	if (dev)
-		core = get_vidc_core_from_dev(dev);
-	if (!core) {
-		dprintk(VIDC_ERR, "Invalid core: %p\n", core);
-		rc = -EINVAL;
-		goto exit;
-	}
-	msm_comm_free_ocmem(core);
-	vidc_hal_core_release(core->device);
-	spin_lock_irqsave(&core->lock, flags);
-	core->state = VIDC_CORE_UNINIT;
-	spin_unlock_irqrestore(&core->lock, flags);
-	msm_comm_unload_fw(core);
-exit:
-	return rc;
-}
-void msm_vidc_crash_shutdown(const struct subsys_desc *subsys)
-{
-	dprintk(VIDC_DBG, "Nothing implemented in crash shutdown\n");
-}
-static struct subsys_desc msm_vidc_subsystem = {
-	.name = "msm_vidc",
-	.dev = NULL,
-	.shutdown = msm_vidc_shutdown,
-	.powerup = msm_vidc_powerup,
-	.ramdump = msm_vidc_ramdump,
-	.crash_shutdown = msm_vidc_crash_shutdown
-};
-int msm_vidc_ssr_init(struct msm_vidc_core *core)
-{
-	int rc = 0;
-	msm_vidc_subsystem.dev = &core->vdev[MSM_VIDC_DECODER].vdev.dev;
-	core->ssr_info.msm_vidc_dev = subsys_register(&msm_vidc_subsystem);
-	if (IS_ERR_OR_NULL(core->ssr_info.msm_vidc_dev)) {
-		dprintk(VIDC_ERR, "msm_vidc Sub System registration failed\n");
-		rc = -ENODEV;
-	}
-	core->ssr_info.msm_vidc_ramdump_dev = create_ramdump_device("msm_vidc",
-			msm_vidc_subsystem.dev);
-	if (!core->ssr_info.msm_vidc_ramdump_dev) {
-		dprintk(VIDC_ERR, "Unable to create msm_vidc ramdump device\n");
-		rc = -ENODEV;
-	}
-	core->ssr_info.ssr_in_progress = false;
-	return rc;
-}
-
-int msm_vidc_ssr_uninit(struct msm_vidc_core *core)
-{
-	subsys_unregister(core->ssr_info.msm_vidc_dev);
-	destroy_ramdump_device(core->ssr_info.msm_vidc_ramdump_dev);
-	return 0;
-}
diff --git a/drivers/media/video/msm_vidc/msm_vidc_ssr.h b/drivers/media/video/msm_vidc/msm_vidc_ssr.h
deleted file mode 100644
index 90f7380..0000000
--- a/drivers/media/video/msm_vidc/msm_vidc_ssr.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
-#ifndef __MSM_VIDC_SSR__
-#define __MSM_VIDC_SSR__
-
-#include <../ramdump.h>
-#include <mach/subsystem_restart.h>
-#include <mach/subsystem_notif.h>
-#include <media/msm_vidc.h>
-#include "msm_vidc_internal.h"
-#include "msm_vidc_common.h"
-#include "msm_vidc_debug.h"
-#include "vidc_hal_api.h"
-int msm_vidc_ssr_init(struct msm_vidc_core *core);
-int msm_vidc_ssr_uninit(struct msm_vidc_core *core);
-
-#endif
diff --git a/drivers/media/video/msm_vidc/vidc_hal.c b/drivers/media/video/msm_vidc/vidc_hal.c
index 207bfe4..10ab0dd 100644
--- a/drivers/media/video/msm_vidc/vidc_hal.c
+++ b/drivers/media/video/msm_vidc/vidc_hal.c
@@ -729,6 +729,7 @@
 		dprintk(VIDC_ERR, "Invalid device");
 		return -ENODEV;
 	}
+	dev->intr_status = 0;
 	enable_irq(dev->hal_data->irq);
 	INIT_LIST_HEAD(&dev->sess_head);
 	spin_lock_init(&dev->read_lock);
@@ -791,10 +792,12 @@
 		dprintk(VIDC_ERR, "invalid device");
 		return -ENODEV;
 	}
-	write_register(dev->hal_data->register_base_addr,
-		VIDC_CPU_CS_SCIACMDARG3, 0, 0);
-	disable_irq_nosync(dev->hal_data->irq);
-	vidc_hal_interface_queues_release(dev);
+	if (dev->hal_client) {
+		write_register(dev->hal_data->register_base_addr,
+				VIDC_CPU_CS_SCIACMDARG3, 0, 0);
+		disable_irq_nosync(dev->hal_data->irq);
+		vidc_hal_interface_queues_release(dev);
+	}
 	dprintk(VIDC_INFO, "HAL exited\n");
 	return 0;
 }
diff --git a/drivers/media/video/msm_vidc/vidc_hal_api.h b/drivers/media/video/msm_vidc/vidc_hal_api.h
index 3b83424..3e70342 100644
--- a/drivers/media/video/msm_vidc/vidc_hal_api.h
+++ b/drivers/media/video/msm_vidc/vidc_hal_api.h
@@ -39,6 +39,7 @@
 #define HAL_BUFFERFLAG_TIMESTAMPINVALID 0x00000100
 #define HAL_BUFFERFLAG_READONLY         0x00000200
 #define HAL_BUFFERFLAG_ENDOFSUBFRAME    0x00000400
+#define HAL_BUFFERFLAG_EOSEQ            0x00200000
 
 #define HAL_DEBUG_MSG_LOW				0x00000001
 #define HAL_DEBUG_MSG_MEDIUM			0x00000002
diff --git a/drivers/media/video/msm_vidc/vidc_hal_interrupt_handler.c b/drivers/media/video/msm_vidc/vidc_hal_interrupt_handler.c
index 3bedb92..8201b0e 100644
--- a/drivers/media/video/msm_vidc/vidc_hal_interrupt_handler.c
+++ b/drivers/media/video/msm_vidc/vidc_hal_interrupt_handler.c
@@ -141,7 +141,7 @@
 static void hal_process_sys_watchdog_timeout(struct hal_device *device)
 {
 	struct msm_vidc_cb_cmd_done cmd_done;
-	disable_irq_nosync(device->hal_data->irq);
+	device->intr_status &= ~VIDC_WRAPPER_INTR_STATUS_A2HWD_BMSK;
 	memset(&cmd_done, 0, sizeof(struct msm_vidc_cb_cmd_done));
 	cmd_done.device_id = device->device_id;
 	device->callback(SYS_WATCHDOG_TIMEOUT, &cmd_done);
@@ -149,7 +149,6 @@
 static void hal_process_sys_error(struct hal_device *device)
 {
 	struct msm_vidc_cb_cmd_done cmd_done;
-	disable_irq_nosync(device->hal_data->irq);
 	memset(&cmd_done, 0, sizeof(struct msm_vidc_cb_cmd_done));
 	cmd_done.device_id = device->device_id;
 	device->callback(SYS_ERROR, &cmd_done);
@@ -177,11 +176,12 @@
 
 	switch (pkt->event_id) {
 	case HFI_EVENT_SYS_ERROR:
-		dprintk(VIDC_INFO, "HFI_EVENT_SYS_ERROR");
+		dprintk(VIDC_ERR, "HFI_EVENT_SYS_ERROR: %d\n",
+			pkt->event_data1);
 		hal_process_sys_error(device);
 		break;
 	case HFI_EVENT_SESSION_ERROR:
-		dprintk(VIDC_INFO, "HFI_EVENT_SESSION_ERROR");
+		dprintk(VIDC_ERR, "HFI_EVENT_SESSION_ERROR");
 		hal_process_session_error(device, pkt);
 		break;
 	case HFI_EVENT_SESSION_SEQUENCE_CHANGED:
@@ -192,7 +192,7 @@
 		dprintk(VIDC_INFO, "HFI_EVENT_SESSION_PROPERTY_CHANGED");
 		break;
 	default:
-		dprintk(VIDC_INFO, "hal_process_event_notify:unkown_event_id");
+		dprintk(VIDC_WARN, "hal_process_event_notify:unkown_event_id");
 		break;
 	}
 }
@@ -825,6 +825,7 @@
 	if ((device->intr_status & VIDC_WRAPPER_INTR_CLEAR_A2HWD_BMSK)) {
 		dprintk(VIDC_ERR, "Received: Watchdog timeout %s", __func__);
 		hal_process_sys_watchdog_timeout(device);
+		return;
 	}
 
 	switch (msg_hdr->packet) {
diff --git a/drivers/media/video/msm_wfd/enc-mfc-subdev.c b/drivers/media/video/msm_wfd/enc-mfc-subdev.c
index 21fc719..aadf5ed 100644
--- a/drivers/media/video/msm_wfd/enc-mfc-subdev.c
+++ b/drivers/media/video/msm_wfd/enc-mfc-subdev.c
@@ -903,25 +903,13 @@
 	struct vcd_property_req_i_frame vcd_property_req_i_frame;
 	struct vcd_property_hdr vcd_property_hdr;
 
-	int rc = 0;
-	switch (type) {
-	case V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_DISABLED:
-		/*So...nothing to do?*/
-		break;
-	case V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_I_FRAME:
-		vcd_property_hdr.prop_id = VCD_I_REQ_IFRAME;
-		vcd_property_hdr.sz = sizeof(struct vcd_property_req_i_frame);
-		vcd_property_req_i_frame.req_i_frame = 1;
+	vcd_property_hdr.prop_id = VCD_I_REQ_IFRAME;
+	vcd_property_hdr.sz = sizeof(struct vcd_property_req_i_frame);
+	vcd_property_req_i_frame.req_i_frame = 1;
 
-		rc = vcd_set_property(client_ctx->vcd_handle,
-				&vcd_property_hdr, &vcd_property_req_i_frame);
-		break;
-	case V4L2_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE_NOT_CODED:
-	default:
-		rc = -ENOTSUPP;
-	}
+	return vcd_set_property(client_ctx->vcd_handle,
+			&vcd_property_hdr, &vcd_property_req_i_frame);
 
-	return rc;
 }
 
 static long venc_set_bitrate(struct video_client_ctx *client_ctx,
@@ -1348,10 +1336,10 @@
 	int level = 0;
 
 	switch (value) {
-	case V4L2_CID_MPEG_QCOM_PERF_LEVEL_PERFORMANCE:
+	case V4L2_CID_MPEG_VIDC_PERF_LEVEL_PERFORMANCE:
 		level = VCD_PERF_LEVEL2;
 		break;
-	case V4L2_CID_MPEG_QCOM_PERF_LEVEL_TURBO:
+	case V4L2_CID_MPEG_VIDC_PERF_LEVEL_TURBO:
 		level = VCD_PERF_LEVEL_TURBO;
 		break;
 	default:
@@ -2304,7 +2292,7 @@
 	case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
 		rc = venc_set_codec_profile(client_ctx, ctrl->id, ctrl->value);
 		break;
-	case V4L2_CID_MPEG_MFC51_VIDEO_FORCE_FRAME_TYPE:
+	case V4L2_CID_MPEG_VIDC_VIDEO_REQUEST_IFRAME:
 		rc = venc_request_frame(client_ctx, ctrl->value);
 		break;
 	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
@@ -2335,7 +2323,7 @@
 		rc = venc_set_multislicing_mode(client_ctx, ctrl->id,
 				ctrl->value);
 		break;
-	case V4L2_CID_MPEG_QCOM_SET_PERF_LEVEL:
+	case V4L2_CID_MPEG_VIDC_SET_PERF_LEVEL:
 		rc = venc_set_max_perf_level(client_ctx, ctrl->value);
 		break;
 	case V4L2_CID_MPEG_VIDC_VIDEO_H264_AU_DELIMITER:
diff --git a/drivers/media/video/msm_wfd/enc-venus-subdev.c b/drivers/media/video/msm_wfd/enc-venus-subdev.c
index 480fe35..73a3d8e 100644
--- a/drivers/media/video/msm_wfd/enc-venus-subdev.c
+++ b/drivers/media/video/msm_wfd/enc-venus-subdev.c
@@ -240,6 +240,18 @@
 	return 0;
 }
 
+static long set_default_properties(struct venc_inst *inst)
+{
+	struct v4l2_control ctrl = {0};
+
+	/* Set the IDR period as 1.  The venus core doesn't give
+	 * the sps/pps for I-frames, only IDR. */
+	ctrl.id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD;
+	ctrl.value = 1;
+
+	return msm_vidc_s_ctrl(inst->vidc_context, &ctrl);
+}
+
 static long venc_open(struct v4l2_subdev *sd, void *arg)
 {
 	struct venc_inst *inst = NULL;
@@ -489,6 +501,9 @@
 
 	inst = (struct venc_inst *)sd->dev_priv;
 
+	if (set_default_properties(inst))
+		WFD_MSG_WARN("Couldn't set default properties\n");
+
 	rc = msm_vidc_streamon(inst->vidc_context, BUF_TYPE_OUTPUT);
 	if (rc) {
 		WFD_MSG_ERR("Failed to streamon vidc's output port");
@@ -803,7 +818,7 @@
 static long venc_set_framerate(struct v4l2_subdev *sd, void *arg)
 {
 	struct venc_inst *inst = NULL;
-	struct v4l2_control ctrl = {0};
+	struct v4l2_streamparm p = {0};
 
 	if (!sd) {
 		WFD_MSG_ERR("Subdevice required for %s\n", __func__);
@@ -814,9 +829,9 @@
 	}
 
 	inst = (struct venc_inst *)sd->dev_priv;
-	ctrl.id = V4L2_CID_MPEG_VIDC_VIDEO_FRAME_RATE;
-	ctrl.value = 30;
-	return msm_vidc_s_ctrl(inst->vidc_context, &ctrl);
+	p.type = BUF_TYPE_INPUT;
+	p.parm.output.timeperframe = *(struct v4l2_fract *)arg;
+	return msm_vidc_s_parm(inst->vidc_context, &p);
 }
 
 static long venc_fill_outbuf(struct v4l2_subdev *sd, void *arg)
@@ -1049,7 +1064,6 @@
 static long venc_set_property(struct v4l2_subdev *sd, void *arg)
 {
 	struct venc_inst *inst = NULL;
-	struct v4l2_control *ctrl = arg;
 
 	if (!sd) {
 		WFD_MSG_ERR("Subdevice required for %s\n", __func__);
@@ -1057,13 +1071,6 @@
 	}
 
 	inst = (struct venc_inst *)sd->dev_priv;
-	if (ctrl->id == V4L2_CID_MPEG_VIDEO_HEADER_MODE) {
-		/* XXX: We don't support this yet, but to prevent unncessary
-		 * target specific code for the client, we'll not error out.
-		 * The client ideally shouldn't notice this */
-		return 0;
-	}
-
 	return msm_vidc_s_ctrl(inst->vidc_context, (struct v4l2_control *)arg);
 }
 
diff --git a/drivers/media/video/v4l2-ctrls.c b/drivers/media/video/v4l2-ctrls.c
index 47b36ae..4012fec 100644
--- a/drivers/media/video/v4l2-ctrls.c
+++ b/drivers/media/video/v4l2-ctrls.c
@@ -252,6 +252,7 @@
 	static const char * const header_mode[] = {
 		"Separate Buffer",
 		"Joined With 1st Frame",
+		"Joined With I-Frames",
 		NULL,
 	};
 	static const char * const multi_slice[] = {
diff --git a/drivers/media/video/vcap_vp.c b/drivers/media/video/vcap_vp.c
index 5161b7b..06891899 100644
--- a/drivers/media/video/vcap_vp.c
+++ b/drivers/media/video/vcap_vp.c
@@ -166,17 +166,22 @@
 void update_nr_value(struct vcap_dev *dev)
 {
 	struct nr_param *par;
+	uint32_t val = 0;
 	par = &dev->nr_param;
 	if (par->mode == NR_MANUAL) {
 		writel_relaxed(par->window << 24 | par->decay_ratio << 20,
 			VCAP_VP_NR_CONFIG);
-		writel_relaxed(par->luma.max_blend_ratio << 24 |
+		if (par->threshold)
+			val = VP_NR_DYNAMIC_THRESHOLD;
+		writel_relaxed(val |
+			par->luma.max_blend_ratio << 24 |
 			par->luma.scale_diff_ratio << 12 |
 			par->luma.diff_limit_ratio << 8  |
 			par->luma.scale_motion_ratio << 4 |
 			par->luma.blend_limit_ratio << 0,
 			VCAP_VP_NR_LUMA_CONFIG);
-		writel_relaxed(par->chroma.max_blend_ratio << 24 |
+		writel_relaxed(val |
+			par->chroma.max_blend_ratio << 24 |
 			par->chroma.scale_diff_ratio << 12 |
 			par->chroma.diff_limit_ratio << 8  |
 			par->chroma.scale_motion_ratio << 4 |
@@ -646,6 +651,9 @@
 	param->decay_ratio = BITS_VALUE(rc, 20, 3);
 
 	rc = readl_relaxed(VCAP_VP_NR_LUMA_CONFIG);
+	param->threshold = NR_THRESHOLD_STATIC;
+	if (BITS_VALUE(rc, 16, 1))
+		param->threshold = NR_THRESHOLD_DYNAMIC;
 	param->luma.max_blend_ratio = BITS_VALUE(rc, 24, 4);
 	param->luma.scale_diff_ratio = BITS_VALUE(rc, 12, 4);
 	param->luma.diff_limit_ratio = BITS_VALUE(rc, 8, 4);
@@ -662,6 +670,7 @@
 
 void s_default_nr_val(struct nr_param *param)
 {
+	param->threshold = NR_THRESHOLD_STATIC;
 	param->window = 10;
 	param->decay_ratio = 0;
 	param->luma.max_blend_ratio = 0;
diff --git a/drivers/media/video/vcap_vp.h b/drivers/media/video/vcap_vp.h
index 2ad5848..70b10c3 100644
--- a/drivers/media/video/vcap_vp.h
+++ b/drivers/media/video/vcap_vp.h
@@ -90,6 +90,7 @@
 
 #define VP_NR_MAX_WINDOW 120
 #define VP_NR_MAX_RATIO  16
+#define VP_NR_DYNAMIC_THRESHOLD 0x000F0000
 
 #define BITS_MASK(start, num_of_bits) \
 	(((1 << (num_of_bits)) - 1) << (start))
diff --git a/drivers/mfd/wcd9xxx-core.c b/drivers/mfd/wcd9xxx-core.c
index fa7c116..d43b399 100644
--- a/drivers/mfd/wcd9xxx-core.c
+++ b/drivers/mfd/wcd9xxx-core.c
@@ -37,6 +37,14 @@
 #define SITAR_I2C_MODE	0x01
 #define CODEC_DT_MAX_PROP_SIZE   40
 #define WCD9XXX_I2C_GSBI_SLAVE_ID "3-000d"
+#define WCD9XXX_I2C_TOP_SLAVE_ADDR	0x0d
+#define WCD9XXX_ANALOG_I2C_SLAVE_ADDR	0x77
+#define WCD9XXX_DIGITAL1_I2C_SLAVE_ADDR	0x66
+#define WCD9XXX_DIGITAL2_I2C_SLAVE_ADDR	0x55
+#define WCD9XXX_I2C_TOP_LEVEL	0
+#define WCD9XXX_I2C_ANALOG	1
+#define WCD9XXX_I2C_DIGITAL_1	2
+#define WCD9XXX_I2C_DIGITAL_2	3
 
 struct wcd9xxx_i2c {
 	struct i2c_client *client;
@@ -644,10 +652,11 @@
 	kfree(wcd9xxx->supplies);
 }
 
-int wcd9xxx_get_intf_type(void)
+enum wcd9xxx_intf_status wcd9xxx_get_intf_type(void)
 {
 	return wcd9xxx_intf;
 }
+
 EXPORT_SYMBOL_GPL(wcd9xxx_get_intf_type);
 
 struct wcd9xxx_i2c *get_i2c_wcd9xxx_device_info(u16 reg)
@@ -768,100 +777,146 @@
 	return wcd9xxx_i2c_write_device(reg, src, bytes);
 }
 
+static int wcd9xxx_i2c_get_client_index(struct i2c_client *client,
+					int *wcd9xx_index)
+{
+	int ret = 0;
+	switch (client->addr) {
+	case WCD9XXX_I2C_TOP_SLAVE_ADDR:
+		*wcd9xx_index = WCD9XXX_I2C_TOP_LEVEL;
+	break;
+	case WCD9XXX_ANALOG_I2C_SLAVE_ADDR:
+		*wcd9xx_index = WCD9XXX_I2C_ANALOG;
+	break;
+	case WCD9XXX_DIGITAL1_I2C_SLAVE_ADDR:
+		*wcd9xx_index = WCD9XXX_I2C_DIGITAL_1;
+	break;
+	case WCD9XXX_DIGITAL2_I2C_SLAVE_ADDR:
+		*wcd9xx_index = WCD9XXX_I2C_DIGITAL_2;
+	break;
+	default:
+		ret = -EINVAL;
+	break;
+	}
+	return ret;
+}
+
 static int __devinit wcd9xxx_i2c_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
-	struct wcd9xxx *wcd9xxx;
-	struct wcd9xxx_pdata *pdata;
+	struct wcd9xxx *wcd9xxx = NULL;
+	struct wcd9xxx_pdata *pdata = NULL;
 	int val = 0;
 	int ret = 0;
 	int i2c_mode = 0;
-	static int device_id;
+	int wcd9xx_index = 0;
 	struct device *dev;
 
-	pr_info("%s\n", __func__);
+	pr_debug("%s: interface status %d\n", __func__, wcd9xxx_intf);
 	if (wcd9xxx_intf == WCD9XXX_INTERFACE_TYPE_SLIMBUS) {
 		dev_dbg(&client->dev, "%s:Codec is detected in slimbus mode\n",
-			 __func__);
+			__func__);
 		return -ENODEV;
-	}
-	if (device_id > 0) {
-		wcd9xxx_modules[device_id++].client = client;
-		dev_dbg(&client->dev, "%s:probe for other slaves\n"
-			"devices of codec\n", __func__);
+	} else if (wcd9xxx_intf == WCD9XXX_INTERFACE_TYPE_I2C) {
+		ret = wcd9xxx_i2c_get_client_index(client, &wcd9xx_index);
+		if (ret != 0)
+			dev_err(&client->dev, "%s: I2C set codec I2C\n"
+				"client failed\n", __func__);
+		else {
+			dev_err(&client->dev, "%s:probe for other slaves\n"
+				"devices of codec I2C slave Addr = %x\n",
+				__func__, client->addr);
+			wcd9xxx_modules[wcd9xx_index].client = client;
+		}
 		return ret;
-	}
-	dev = &client->dev;
-	if (client->dev.of_node) {
-		dev_dbg(&client->dev, "%s:Platform data from device tree\n",
-			__func__);
-		pdata = wcd9xxx_populate_dt_pdata(&client->dev);
-		client->dev.platform_data = pdata;
-	} else {
-		dev_dbg(&client->dev, "%s:Platform data from board file\n",
-			__func__);
-		pdata = client->dev.platform_data;
-	}
-	wcd9xxx = kzalloc(sizeof(struct wcd9xxx), GFP_KERNEL);
-	if (wcd9xxx == NULL) {
-		pr_err("%s: error, allocation failed\n", __func__);
-		ret = -ENOMEM;
-		goto fail;
-	}
+	} else if (wcd9xxx_intf == WCD9XXX_INTERFACE_TYPE_PROBING) {
+		dev = &client->dev;
+		if (client->dev.of_node) {
+			dev_dbg(&client->dev, "%s:Platform data\n"
+				"from device tree\n", __func__);
+			pdata = wcd9xxx_populate_dt_pdata(&client->dev);
+			client->dev.platform_data = pdata;
+		} else {
+			dev_dbg(&client->dev, "%s:Platform data from\n"
+				"board file\n", __func__);
+			pdata = client->dev.platform_data;
+		}
+		wcd9xxx = kzalloc(sizeof(struct wcd9xxx), GFP_KERNEL);
+		if (wcd9xxx == NULL) {
+			pr_err("%s: error, allocation failed\n", __func__);
+			ret = -ENOMEM;
+			goto fail;
+		}
 
-	if (!pdata) {
-		dev_dbg(&client->dev, "no platform data?\n");
-		ret = -EINVAL;
-		goto fail;
-	}
-	if (i2c_check_functionality(client->adapter, I2C_FUNC_I2C) == 0) {
-		dev_dbg(&client->dev, "can't talk I2C?\n");
-		ret = -EIO;
-		goto fail;
-	}
-	dev_set_drvdata(&client->dev, wcd9xxx);
-	wcd9xxx->dev = &client->dev;
-	wcd9xxx->reset_gpio = pdata->reset_gpio;
-	ret = wcd9xxx_enable_supplies(wcd9xxx, pdata);
-	if (ret) {
-		pr_err("%s: Fail to enable Codec supplies\n", __func__);
-		goto err_codec;
-	}
+		if (!pdata) {
+			dev_dbg(&client->dev, "no platform data?\n");
+			ret = -EINVAL;
+			goto fail;
+		}
+		if (i2c_check_functionality(client->adapter,
+					    I2C_FUNC_I2C) == 0) {
+			dev_dbg(&client->dev, "can't talk I2C?\n");
+			ret = -EIO;
+			goto fail;
+		}
+		dev_set_drvdata(&client->dev, wcd9xxx);
+		wcd9xxx->dev = &client->dev;
+		wcd9xxx->reset_gpio = pdata->reset_gpio;
+		if (client->dev.of_node)
+			wcd9xxx->mclk_rate = pdata->mclk_rate;
+		ret = wcd9xxx_enable_supplies(wcd9xxx, pdata);
+		if (ret) {
+			pr_err("%s: Fail to enable Codec supplies\n",
+			       __func__);
+			goto err_codec;
+		}
 
-	usleep_range(5, 5);
-	ret = wcd9xxx_reset(wcd9xxx);
-	if (ret) {
-		pr_err("%s: Resetting Codec failed\n", __func__);
+		usleep_range(5, 5);
+		ret = wcd9xxx_reset(wcd9xxx);
+		if (ret) {
+			pr_err("%s: Resetting Codec failed\n", __func__);
 		goto err_supplies;
-	}
-	wcd9xxx_modules[device_id++].client = client;
+		}
 
-	wcd9xxx->read_dev = wcd9xxx_i2c_read;
-	wcd9xxx->write_dev = wcd9xxx_i2c_write;
-	if (!wcd9xxx->dev->of_node) {
-		wcd9xxx->irq = pdata->irq;
-		wcd9xxx->irq_base = pdata->irq_base;
-	}
+		ret = wcd9xxx_i2c_get_client_index(client, &wcd9xx_index);
+		if (ret != 0) {
+			pr_err("%s:Set codec I2C client failed\n", __func__);
+			goto err_supplies;
+		}
 
-	ret = wcd9xxx_device_init(wcd9xxx);
-	if (ret) {
-		pr_err("%s: error, initializing device failed\n", __func__);
-		goto err_device_init;
-	}
+		wcd9xxx_modules[wcd9xx_index].client = client;
+		wcd9xxx->read_dev = wcd9xxx_i2c_read;
+		wcd9xxx->write_dev = wcd9xxx_i2c_write;
+		if (!wcd9xxx->dev->of_node) {
+			wcd9xxx->irq = pdata->irq;
+			wcd9xxx->irq_base = pdata->irq_base;
+		}
 
-	if ((wcd9xxx->idbyte[0] == 0x2) || (wcd9xxx->idbyte[0] == 0x1))
-		i2c_mode = TABLA_I2C_MODE;
-	else if (wcd9xxx->idbyte[0] == 0x0)
-		i2c_mode = SITAR_I2C_MODE;
+		ret = wcd9xxx_device_init(wcd9xxx);
+		if (ret) {
+			pr_err("%s: error, initializing device failed\n",
+			       __func__);
+			goto err_device_init;
+		}
 
-	ret = wcd9xxx_read(wcd9xxx, WCD9XXX_A_CHIP_STATUS, 1, &val, 0);
+		if ((wcd9xxx->idbyte[0] == 0x2) || (wcd9xxx->idbyte[0] == 0x1))
+			i2c_mode = TABLA_I2C_MODE;
+		else if (wcd9xxx->idbyte[0] == 0x0)
+			i2c_mode = SITAR_I2C_MODE;
 
-	if ((ret < 0) || (val != i2c_mode))
-		pr_err("failed to read the wcd9xxx status ret = %d\n", ret);
+		ret = wcd9xxx_read(wcd9xxx, WCD9XXX_A_CHIP_STATUS, 1, &val, 0);
+
+		if ((ret < 0) || (val != i2c_mode))
+			pr_err("failed to read the wcd9xxx status ret = %d\n",
+			       ret);
 
 	wcd9xxx_intf = WCD9XXX_INTERFACE_TYPE_I2C;
 
-	return ret;
+		return ret;
+	} else
+		pr_err("%s: I2C probe in wrong state\n", __func__);
+
+
 err_device_init:
 	wcd9xxx_free_reset(wcd9xxx);
 err_supplies:
@@ -1087,6 +1142,7 @@
 	int ret, i;
 	char **codec_supplies;
 	u32 num_of_supplies = 0;
+	u32 mclk_rate = 0;
 
 	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata) {
@@ -1130,6 +1186,19 @@
 		goto err;
 	}
 	dev_dbg(dev, "%s: reset gpio %d", __func__, pdata->reset_gpio);
+	ret = of_property_read_u32(dev->of_node,
+				   "qcom,cdc-mclk-clk-rate",
+				   &mclk_rate);
+	if (ret) {
+		dev_err(dev, "Looking up %s property in\n"
+			"node %s failed",
+			"qcom,cdc-mclk-clk-rate",
+			dev->of_node->full_name);
+		devm_kfree(dev, pdata);
+		ret = -EINVAL;
+		goto err;
+	}
+	pdata->mclk_rate = mclk_rate;
 	return pdata;
 err:
 	devm_kfree(dev, pdata);
@@ -1162,6 +1231,11 @@
 	struct wcd9xxx_pdata *pdata;
 	int ret = 0;
 
+	if (wcd9xxx_intf == WCD9XXX_INTERFACE_TYPE_I2C) {
+		dev_dbg(&slim->dev, "%s:Codec is detected in I2C mode\n",
+			__func__);
+		return -ENODEV;
+	}
 	if (slim->dev.of_node) {
 		dev_info(&slim->dev, "Platform data from device tree\n");
 		pdata = wcd9xxx_populate_dt_pdata(&slim->dev);
@@ -1201,6 +1275,7 @@
 	slim_set_clientdata(slim, wcd9xxx);
 	wcd9xxx->reset_gpio = pdata->reset_gpio;
 	wcd9xxx->dev = &slim->dev;
+	wcd9xxx->mclk_rate = pdata->mclk_rate;
 
 	ret = wcd9xxx_enable_supplies(wcd9xxx, pdata);
 	if (ret)
@@ -1477,11 +1552,6 @@
 	.suspend = wcd9xxx_slim_suspend,
 };
 
-#define WCD9XXX_I2C_TOP_LEVEL	0
-#define WCD9XXX_I2C_ANALOG	1
-#define WCD9XXX_I2C_DIGITAL_1	2
-#define WCD9XXX_I2C_DIGITAL_2	3
-
 static struct i2c_device_id wcd9xxx_id_table[] = {
 	{"wcd9xxx-i2c", WCD9XXX_I2C_TOP_LEVEL},
 	{"wcd9xxx-i2c", WCD9XXX_I2C_ANALOG},
@@ -1528,6 +1598,8 @@
 {
 	int ret1, ret2, ret3, ret4, ret5, ret6, ret7;
 
+	wcd9xxx_intf = WCD9XXX_INTERFACE_TYPE_PROBING;
+
 	ret1 = slim_driver_register(&tabla_slim_driver);
 	if (ret1 != 0)
 		pr_err("Failed to register tabla SB driver: %d\n", ret1);
diff --git a/drivers/misc/isa1200.c b/drivers/misc/isa1200.c
index 6c3e787..604ffd7 100644
--- a/drivers/misc/isa1200.c
+++ b/drivers/misc/isa1200.c
@@ -42,7 +42,7 @@
 	struct hrtimer timer;
 	struct timed_output_dev dev;
 	struct work_struct work;
-	spinlock_t lock;
+	struct mutex lock;
 	unsigned int enable;
 	unsigned int period_ns;
 	bool is_len_gpio_valid;
@@ -216,9 +216,8 @@
 {
 	struct isa1200_chip *haptic = container_of(dev, struct isa1200_chip,
 					dev);
-	unsigned long flags;
 
-	spin_lock_irqsave(&haptic->lock, flags);
+	mutex_lock(&haptic->lock);
 	hrtimer_cancel(&haptic->timer);
 	if (value == 0)
 		haptic->enable = 0;
@@ -230,7 +229,7 @@
 			ktime_set(value / 1000, (value % 1000) * 1000000),
 			HRTIMER_MODE_REL);
 	}
-	spin_unlock_irqrestore(&haptic->lock, flags);
+	mutex_unlock(&haptic->lock);
 	schedule_work(&haptic->work);
 }
 
@@ -501,7 +500,7 @@
 		}
 	}
 
-	spin_lock_init(&haptic->lock);
+	mutex_init(&haptic->lock);
 	INIT_WORK(&haptic->work, isa1200_chip_work);
 	haptic->clk_on = false;
 
@@ -591,6 +590,7 @@
 hen_gpio_fail:
 	timed_output_dev_unregister(&haptic->dev);
 timed_reg_fail:
+	mutex_destroy(&haptic->lock);
 	if (pdata->power_on)
 		pdata->power_on(0);
 pwr_up_fail:
@@ -637,6 +637,9 @@
 				ISA1200_HCTRL1_RESET);
 
 
+	/* destroy mutex */
+	mutex_destroy(&haptic->lock);
+
 	/* power-off the chip */
 	if (haptic->pdata->regulator_info) {
 		isa1200_reg_power(haptic, false);
diff --git a/drivers/misc/smsc_hub.c b/drivers/misc/smsc_hub.c
index bde25d9..31f18ed 100644
--- a/drivers/misc/smsc_hub.c
+++ b/drivers/misc/smsc_hub.c
@@ -16,6 +16,9 @@
 #include <linux/regulator/consumer.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
+#include <linux/of_device.h>
+#include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
 #include <linux/smsc3503.h>
@@ -28,12 +31,16 @@
 SMSC3503_I2C_ADDR, I2C_CLIENT_END };
 
 struct hsic_hub {
-	struct regulator *hsic_hub_reg;
 	struct device *dev;
+	struct smsc_hub_platform_data *pdata;
 	struct i2c_client *client;
 	struct msm_xo_voter *xo_handle;
+	struct clk		*ref_clk;
+	struct regulator	*hsic_hub_reg;
+	struct regulator	*int_pad_reg, *hub_vbus_reg;
 };
 static struct hsic_hub *smsc_hub;
+static struct platform_driver smsc_hub_driver;
 
 /* APIs for setting/clearing bits and for reading/writing values */
 static inline int hsic_hub_get_u8(struct i2c_client *client, u8 reg)
@@ -150,16 +157,208 @@
 	.id_table = hsic_hub_id,
 };
 
+static int msm_hsic_hub_init_clock(struct hsic_hub *hub, int init)
+{
+	int ret;
+
+	if (!init) {
+		if (!IS_ERR(hub->ref_clk))
+			clk_disable_unprepare(hub->ref_clk);
+		else
+			msm_xo_put(smsc_hub->xo_handle);
+
+		return 0;
+	}
+
+	hub->ref_clk = devm_clk_get(hub->dev, "ref_clk");
+	if (IS_ERR(hub->ref_clk)) {
+		dev_dbg(hub->dev, "failed to get ref_clk\n");
+
+		/* In the absence of dedicated ref_clk, xo clocks the HUB */
+		smsc_hub->xo_handle = msm_xo_get(MSM_XO_TCXO_D1, "hsic_hub");
+		if (IS_ERR(smsc_hub->xo_handle)) {
+			dev_err(hub->dev, "not able to get the handle\n"
+						 "for TCXO D1 buffer\n");
+			return PTR_ERR(smsc_hub->xo_handle);
+		}
+
+		ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_ON);
+		if (ret) {
+			dev_err(hub->dev, "failed to vote for TCXO\n"
+				"D1 buffer\n");
+			msm_xo_put(smsc_hub->xo_handle);
+			return ret;
+		}
+	} else {
+		ret = clk_prepare_enable(hub->ref_clk);
+		if (ret)
+			dev_err(hub->dev, "clk_enable failed for ref_clk\n");
+	}
+
+	return ret;
+}
+#define HSIC_HUB_INT_VOL_MIN	1800000 /* uV */
+#define HSIC_HUB_INT_VOL_MAX	2950000 /* uV */
+static int msm_hsic_hub_init_gpio(struct hsic_hub *hub, int init)
+{
+	int ret;
+	struct smsc_hub_platform_data *pdata = hub->pdata;
+
+	if (!init) {
+		if (!IS_ERR(smsc_hub->int_pad_reg)) {
+			regulator_disable(smsc_hub->int_pad_reg);
+			regulator_set_voltage(smsc_hub->int_pad_reg, 0,
+						HSIC_HUB_INT_VOL_MAX);
+		}
+		return 0;
+	}
+
+	ret = devm_gpio_request(hub->dev, pdata->hub_reset, "HSIC_HUB_RESET");
+	if (ret < 0) {
+		dev_err(hub->dev, "gpio request failed for GPIO%d\n",
+							pdata->hub_reset);
+		return ret;
+	}
+
+	if (pdata->refclk_gpio) {
+		ret = devm_gpio_request(hub->dev, pdata->refclk_gpio,
+							 "HSIC_HUB_CLK");
+		if (ret < 0)
+			dev_err(hub->dev, "gpio request failed (CLK GPIO)\n");
+	}
+
+	if (pdata->int_gpio) {
+		ret = devm_gpio_request(hub->dev, pdata->int_gpio,
+							 "HSIC_HUB_INT");
+		if (ret < 0) {
+			dev_err(hub->dev, "gpio request failed (INT GPIO)\n");
+			return ret;
+		}
+
+		/* Enable LDO if required for external pull-up */
+		smsc_hub->int_pad_reg = devm_regulator_get(hub->dev, "hub_int");
+		if (IS_ERR(smsc_hub->int_pad_reg)) {
+			dev_dbg(hub->dev, "unable to get ext hub_int reg\n");
+		} else {
+			ret = regulator_set_voltage(smsc_hub->int_pad_reg,
+						HSIC_HUB_INT_VOL_MIN,
+						HSIC_HUB_INT_VOL_MAX);
+			if (ret) {
+				dev_err(hub->dev, "unable to set the voltage\n"
+						" for hsic hub int reg\n");
+				return ret;
+			}
+			ret = regulator_enable(smsc_hub->int_pad_reg);
+			if (ret) {
+				dev_err(hub->dev, "unable to enable int reg\n");
+				regulator_set_voltage(smsc_hub->int_pad_reg, 0,
+							HSIC_HUB_INT_VOL_MAX);
+				return ret;
+			}
+		}
+	}
+
+	return 0;
+}
+
 #define HSIC_HUB_VDD_VOL_MIN	1650000 /* uV */
 #define HSIC_HUB_VDD_VOL_MAX	1950000 /* uV */
 #define HSIC_HUB_VDD_LOAD	36000	/* uA */
+static int msm_hsic_hub_init_vdd(struct hsic_hub *hub, int init)
+{
+	int ret;
+
+	if (!init) {
+		if (!IS_ERR(smsc_hub->hsic_hub_reg)) {
+			regulator_disable(smsc_hub->hsic_hub_reg);
+			regulator_set_optimum_mode(smsc_hub->hsic_hub_reg, 0);
+			regulator_set_voltage(smsc_hub->hsic_hub_reg, 0,
+							HSIC_HUB_VDD_VOL_MAX);
+		}
+		return 0;
+	}
+
+	smsc_hub->hsic_hub_reg = devm_regulator_get(hub->dev, "EXT_HUB_VDDIO");
+	if (IS_ERR(smsc_hub->hsic_hub_reg)) {
+		dev_dbg(hub->dev, "unable to get ext hub vddcx\n");
+	} else {
+		ret = regulator_set_voltage(smsc_hub->hsic_hub_reg,
+				HSIC_HUB_VDD_VOL_MIN,
+				HSIC_HUB_VDD_VOL_MAX);
+		if (ret) {
+			dev_err(hub->dev, "unable to set the voltage\n"
+						"for hsic hub reg\n");
+			return ret;
+		}
+
+		ret = regulator_set_optimum_mode(smsc_hub->hsic_hub_reg,
+					HSIC_HUB_VDD_LOAD);
+		if (ret < 0) {
+			dev_err(hub->dev, "Unable to set mode of VDDCX\n");
+			goto reg_optimum_mode_fail;
+		}
+
+		ret = regulator_enable(smsc_hub->hsic_hub_reg);
+		if (ret) {
+			dev_err(hub->dev, "unable to enable ext hub vddcx\n");
+			goto reg_enable_fail;
+		}
+	}
+
+	return 0;
+
+reg_enable_fail:
+	regulator_set_optimum_mode(smsc_hub->hsic_hub_reg, 0);
+reg_optimum_mode_fail:
+	regulator_set_voltage(smsc_hub->hsic_hub_reg, 0,
+				HSIC_HUB_VDD_VOL_MAX);
+
+	return ret;
+}
+struct smsc_hub_platform_data *msm_hub_dt_to_pdata(
+				struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct smsc_hub_platform_data *pdata;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&pdev->dev, "unable to allocate platform data\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	pdata->hub_reset = of_get_named_gpio(node, "smsc,reset-gpio", 0);
+	if (pdata->hub_reset < 0)
+		return ERR_PTR(pdata->hub_reset);
+
+	pdata->refclk_gpio = of_get_named_gpio(node, "smsc,refclk-gpio", 0);
+	if (pdata->refclk_gpio < 0)
+		pdata->refclk_gpio = 0;
+
+	pdata->int_gpio = of_get_named_gpio(node, "smsc,int-gpio", 0);
+	if (pdata->int_gpio < 0)
+		pdata->int_gpio = 0;
+
+	return pdata;
+}
+
 static int __devinit smsc_hub_probe(struct platform_device *pdev)
 {
 	int ret = 0;
 	const struct smsc_hub_platform_data *pdata;
+	struct device_node *node = pdev->dev.of_node;
 	struct i2c_adapter *i2c_adap;
 	struct i2c_board_info i2c_info;
 
+	if (pdev->dev.of_node) {
+		dev_dbg(&pdev->dev, "device tree enabled\n");
+		pdev->dev.platform_data = msm_hub_dt_to_pdata(pdev);
+		if (IS_ERR(pdev->dev.platform_data))
+			return PTR_ERR(pdev->dev.platform_data);
+
+		dev_set_name(&pdev->dev, smsc_hub_driver.driver.name);
+	}
+
 	if (!pdev->dev.platform_data) {
 		dev_err(&pdev->dev, "No platform data\n");
 		return -ENODEV;
@@ -169,59 +368,34 @@
 	if (!pdata->hub_reset)
 		return -EINVAL;
 
-	smsc_hub = kzalloc(sizeof(*smsc_hub), GFP_KERNEL);
+	smsc_hub = devm_kzalloc(&pdev->dev, sizeof(*smsc_hub), GFP_KERNEL);
 	if (!smsc_hub)
 		return -ENOMEM;
 
-	smsc_hub->hsic_hub_reg = regulator_get(&pdev->dev, "EXT_HUB_VDDIO");
-	if (IS_ERR(smsc_hub->hsic_hub_reg)) {
-		dev_err(&pdev->dev, "unable to get ext hub vddcx\n");
-		ret = PTR_ERR(smsc_hub->hsic_hub_reg);
-		goto free_mem;
+	smsc_hub->dev = &pdev->dev;
+	smsc_hub->pdata = pdev->dev.platform_data;
+
+	smsc_hub->hub_vbus_reg = devm_regulator_get(&pdev->dev, "hub_vbus");
+	ret = PTR_ERR(smsc_hub->hub_vbus_reg);
+	if (ret == -EPROBE_DEFER) {
+		dev_dbg(&pdev->dev, "failed to get hub_vbus\n");
+		return ret;
 	}
 
-	ret = gpio_request(pdata->hub_reset, "HSIC_HUB_RESET_GPIO");
-	if (ret < 0) {
-		dev_err(&pdev->dev, "gpio request failed for GPIO%d\n",
-							pdata->hub_reset);
-		goto gpio_req_fail;
-	}
-
-	ret = regulator_set_voltage(smsc_hub->hsic_hub_reg,
-			HSIC_HUB_VDD_VOL_MIN,
-			HSIC_HUB_VDD_VOL_MAX);
+	ret = msm_hsic_hub_init_vdd(smsc_hub, 1);
 	if (ret) {
-		dev_err(&pdev->dev, "unable to set the voltage"
-				"for hsic hub reg\n");
-		goto reg_set_voltage_fail;
+		dev_err(&pdev->dev, "failed to init hub VDD\n");
+		return ret;
 	}
-
-	ret = regulator_set_optimum_mode(smsc_hub->hsic_hub_reg,
-				HSIC_HUB_VDD_LOAD);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Unable to set optimum mode of regulator:"
-							"VDDCX\n");
-		goto reg_optimum_mode_fail;
-	}
-
-	ret = regulator_enable(smsc_hub->hsic_hub_reg);
+	ret = msm_hsic_hub_init_clock(smsc_hub, 1);
 	if (ret) {
-		dev_err(&pdev->dev, "unable to enable ext hub vddcx\n");
-		goto reg_enable_fail;
+		dev_err(&pdev->dev, "failed to init hub clock\n");
+		goto uninit_vdd;
 	}
-
-	smsc_hub->xo_handle = msm_xo_get(MSM_XO_TCXO_D1, "hsic_hub");
-	if (IS_ERR(smsc_hub->xo_handle)) {
-		dev_err(&pdev->dev, "not able to get the handle"
-					 "for TCXO D1 buffer\n");
-			goto disable_regulator;
-	}
-
-	ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_ON);
+	ret = msm_hsic_hub_init_gpio(smsc_hub, 1);
 	if (ret) {
-		dev_err(&pdev->dev, "failed to vote for TCXO"
-			"D1 buffer\n");
-		goto xo_vote_fail;
+		dev_err(&pdev->dev, "failed to init hub gpios\n");
+		goto uninit_clock;
 	}
 
 	gpio_direction_output(pdata->hub_reset, 0);
@@ -231,6 +405,20 @@
 	udelay(5);
 	gpio_direction_output(pdata->hub_reset, 1);
 
+	ret = of_platform_populate(node, NULL, NULL, &pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add child node, ret=%d\n", ret);
+		goto uninit_gpio;
+	}
+
+	if (!IS_ERR(smsc_hub->hub_vbus_reg)) {
+		ret = regulator_enable(smsc_hub->hub_vbus_reg);
+		if (ret) {
+			dev_err(&pdev->dev, "unable to enable hub_vbus\n");
+			goto uninit_gpio;
+		}
+	}
+
 	ret = i2c_add_driver(&hsic_hub_driver);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to add I2C hsic_hub_driver\n");
@@ -261,21 +449,12 @@
 
 	return 0;
 
-xo_vote_fail:
-	msm_xo_put(smsc_hub->xo_handle);
-disable_regulator:
-	regulator_disable(smsc_hub->hsic_hub_reg);
-reg_enable_fail:
-	regulator_set_optimum_mode(smsc_hub->hsic_hub_reg, 0);
-reg_optimum_mode_fail:
-	regulator_set_voltage(smsc_hub->hsic_hub_reg, 0,
-				HSIC_HUB_VDD_VOL_MIN);
-reg_set_voltage_fail:
-	gpio_free(pdata->hub_reset);
-gpio_req_fail:
-	regulator_put(smsc_hub->hsic_hub_reg);
-free_mem:
-	kfree(smsc_hub);
+uninit_gpio:
+	msm_hsic_hub_init_gpio(smsc_hub, 0);
+uninit_clock:
+	msm_hsic_hub_init_clock(smsc_hub, 0);
+uninit_vdd:
+	msm_hsic_hub_init_vdd(smsc_hub, 0);
 
 	return ret;
 }
@@ -291,15 +470,11 @@
 		i2c_del_driver(&hsic_hub_driver);
 	}
 	pm_runtime_disable(&pdev->dev);
-	msm_xo_put(smsc_hub->xo_handle);
 
-	regulator_disable(smsc_hub->hsic_hub_reg);
-	regulator_set_optimum_mode(smsc_hub->hsic_hub_reg, 0);
-	regulator_set_voltage(smsc_hub->hsic_hub_reg, 0,
-				HSIC_HUB_VDD_VOL_MIN);
-	gpio_free(pdata->hub_reset);
-	regulator_put(smsc_hub->hsic_hub_reg);
-	kfree(smsc_hub);
+	regulator_disable(smsc_hub->hub_vbus_reg);
+	msm_hsic_hub_init_gpio(smsc_hub, 0);
+	msm_hsic_hub_init_clock(smsc_hub, 0);
+	msm_hsic_hub_init_vdd(smsc_hub, 0);
 
 	return 0;
 }
@@ -314,24 +489,32 @@
 
 static int smsc_hub_lpm_enter(struct device *dev)
 {
-	int ret;
+	int ret = 0;
 
-	ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_OFF);
-	if (ret) {
-		pr_err("%s: failed to devote for TCXO"
-			"D1 buffer%d\n", __func__, ret);
+	if (!IS_ERR(smsc_hub->ref_clk)) {
+		clk_disable_unprepare(smsc_hub->ref_clk);
+	} else {
+		ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_OFF);
+		if (ret) {
+			pr_err("%s: failed to devote for TCXO\n"
+				"D1 buffer%d\n", __func__, ret);
+		}
 	}
 	return ret;
 }
 
 static int smsc_hub_lpm_exit(struct device *dev)
 {
-	int ret;
+	int ret = 0;
 
-	ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_ON);
-	if (ret) {
-		pr_err("%s: failed to vote for TCXO"
-			"D1 buffer%d\n", __func__, ret);
+	if (!IS_ERR(smsc_hub->ref_clk)) {
+		clk_prepare_enable(smsc_hub->ref_clk);
+	} else {
+		ret = msm_xo_mode_vote(smsc_hub->xo_handle, MSM_XO_MODE_ON);
+		if (ret) {
+			pr_err("%s: failed to vote for TCXO\n"
+				"D1 buffer%d\n", __func__, ret);
+		}
 	}
 	return ret;
 }
@@ -345,6 +528,13 @@
 };
 #endif
 
+static const struct of_device_id hsic_hub_dt_match[] = {
+	{ .compatible = "qcom,hsic-smsc-hub",
+	},
+	{}
+};
+MODULE_DEVICE_TABLE(of, hsic_hub_dt_match);
+
 static struct platform_driver smsc_hub_driver = {
 	.driver = {
 		.name = "msm_smsc_hub",
@@ -352,13 +542,15 @@
 #ifdef CONFIG_PM
 		.pm = &smsc_hub_dev_pm_ops,
 #endif
+		.of_match_table = hsic_hub_dt_match,
 	},
+	.probe = smsc_hub_probe,
 	.remove = smsc_hub_remove,
 };
 
 static int __init smsc_hub_init(void)
 {
-	return platform_driver_probe(&smsc_hub_driver, smsc_hub_probe);
+	return platform_driver_register(&smsc_hub_driver);
 }
 
 static void __exit smsc_hub_exit(void)
diff --git a/drivers/misc/ti_drv2667.c b/drivers/misc/ti_drv2667.c
index 554799c..d06d336 100644
--- a/drivers/misc/ti_drv2667.c
+++ b/drivers/misc/ti_drv2667.c
@@ -46,7 +46,7 @@
 #define DRV2667_FIFO_CHUNK_MS	10
 #define DRV2667_BYTES_PER_MS	8
 
-#define DRV2667_WAV_SEQ_ID_IDX		1
+#define DRV2667_WAV_SEQ_ID_IDX		0
 #define DRV2667_WAV_SEQ_REP_IDX		6
 #define DRV2667_WAV_SEQ_FREQ_IDX	8
 #define DRV2667_WAV_SEQ_FREQ_MIN	8
@@ -128,11 +128,19 @@
 	data = container_of(work, struct drv2667_data, work);
 
 	if (data->mode == WAV_SEQ_MODE) {
-		if (data->runtime_left)
-			val = data->cntl2_val | DRV2667_GO_MASK;
-		else
-			val = data->cntl2_val & ~DRV2667_GO_MASK;
+		/* clear go bit */
+		val = data->cntl2_val & ~DRV2667_GO_MASK;
 		rc = drv2667_write_reg(data->client, DRV2667_CNTL2_REG, val);
+		if (rc < 0) {
+			dev_err(&data->client->dev, "i2c send msg failed\n");
+			return;
+		}
+		/* restart wave if runtime is left */
+		if (data->runtime_left) {
+			val = data->cntl2_val | DRV2667_GO_MASK;
+			rc = drv2667_write_reg(data->client,
+						DRV2667_CNTL2_REG, val);
+		}
 	} else if (data->mode == FIFO_MODE) {
 		/* data is played at 8khz */
 		if (data->runtime_left < data->time_chunk_ms)
@@ -577,8 +585,9 @@
 			goto vreg_off;
 
 		/* program waveform sequence */
-		for (reg = 0, i = 1; i < DRV2667_WAV_SEQ_LEN - 1; i++, reg++) {
-			rc = drv2667_write_reg(client, reg, pdata->wav_seq[i]);
+		for (reg = 0, i = 0; i < DRV2667_WAV_SEQ_LEN - 1; i++, reg++) {
+			rc = drv2667_write_reg(client, reg,
+						pdata->wav_seq[i+1]);
 			if (rc < 0)
 				goto vreg_off;
 		}
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index 76e9b9c..0b5449e 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -136,6 +136,10 @@
 module_param(perdev_minors, int, 0444);
 MODULE_PARM_DESC(perdev_minors, "Minors numbers to allocate per device");
 
+static inline int mmc_blk_part_switch(struct mmc_card *card,
+				      struct mmc_blk_data *md);
+static int get_card_status(struct mmc_card *card, u32 *status, int retries);
+
 static inline void mmc_blk_clear_packed(struct mmc_queue_req *mqrq)
 {
 	mqrq->packed_cmd = MMC_PACKED_NONE;
@@ -463,6 +467,38 @@
 	return ERR_PTR(err);
 }
 
+static int ioctl_rpmb_card_status_poll(struct mmc_card *card, u32 *status,
+				       u32 retries_max)
+{
+	int err;
+	u32 retry_count = 0;
+
+	if (!status || !retries_max)
+		return -EINVAL;
+
+	do {
+		err = get_card_status(card, status, 5);
+		if (err)
+			break;
+
+		if (!R1_STATUS(*status) &&
+				(R1_CURRENT_STATE(*status) != R1_STATE_PRG))
+			break; /* RPMB programming operation complete */
+
+		/*
+		 * Rechedule to give the MMC device a chance to continue
+		 * processing the previous command without being polled too
+		 * frequently.
+		 */
+		usleep_range(1000, 5000);
+	} while (++retry_count < retries_max);
+
+	if (retry_count == retries_max)
+		err = -EPERM;
+
+	return err;
+}
+
 static int mmc_blk_ioctl_cmd(struct block_device *bdev,
 	struct mmc_ioc_cmd __user *ic_ptr)
 {
@@ -474,6 +510,8 @@
 	struct mmc_request mrq = {NULL};
 	struct scatterlist sg;
 	int err;
+	int is_rpmb = false;
+	u32 status = 0;
 
 	/*
 	 * The caller must have CAP_SYS_RAWIO, and must be calling this on the
@@ -493,6 +531,9 @@
 		goto cmd_done;
 	}
 
+	if (md->area_type & MMC_BLK_DATA_AREA_RPMB)
+		is_rpmb = true;
+
 	card = md->queue.card;
 	if (IS_ERR(card)) {
 		err = PTR_ERR(card);
@@ -543,12 +584,23 @@
 
 	mmc_claim_host(card->host);
 
+	err = mmc_blk_part_switch(card, md);
+	if (err)
+		goto cmd_rel_host;
+
 	if (idata->ic.is_acmd) {
 		err = mmc_app_cmd(card->host, card);
 		if (err)
 			goto cmd_rel_host;
 	}
 
+	if (is_rpmb) {
+		err = mmc_set_blockcount(card, data.blocks,
+			idata->ic.write_flag & (1 << 31));
+		if (err)
+			goto cmd_rel_host;
+	}
+
 	mmc_wait_for_req(card->host, &mrq);
 
 	if (cmd.error) {
@@ -584,6 +636,18 @@
 		}
 	}
 
+	if (is_rpmb) {
+		/*
+		 * Ensure RPMB command has completed by polling CMD13
+		 * "Send Status".
+		 */
+		err = ioctl_rpmb_card_status_poll(card, &status, 5);
+		if (err)
+			dev_err(mmc_dev(card->host),
+					"%s: Card Status=0x%08X, error %d\n",
+					__func__, status, err);
+	}
+
 cmd_rel_host:
 	mmc_release_host(card->host);
 
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index f91ba89..89f834a 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -2214,6 +2214,20 @@
 }
 EXPORT_SYMBOL(mmc_set_blocklen);
 
+int mmc_set_blockcount(struct mmc_card *card, unsigned int blockcount,
+			bool is_rel_write)
+{
+	struct mmc_command cmd = {0};
+
+	cmd.opcode = MMC_SET_BLOCK_COUNT;
+	cmd.arg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.arg |= 1 << 31;
+	cmd.flags = MMC_RSP_SPI_R1 | MMC_RSP_R1 | MMC_CMD_AC;
+	return mmc_wait_for_cmd(card->host, &cmd, 5);
+}
+EXPORT_SYMBOL(mmc_set_blockcount);
+
 static void mmc_hw_reset_for_init(struct mmc_host *host)
 {
 	if (!(host->caps & MMC_CAP_HW_RESET) || !host->ops->hw_reset)
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index d30f10f..751ba75 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -4,6 +4,7 @@
  *  Copyright (C) 2003 Russell King, All Rights Reserved.
  *  Copyright (C) 2007-2008 Pierre Ossman
  *  Copyright (C) 2010 Linus Walleij
+ *  Copyright (c) 2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -362,6 +363,169 @@
 }
 
 EXPORT_SYMBOL(mmc_alloc_host);
+
+static ssize_t show_enable(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+
+	if (!host)
+		return -EINVAL;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", mmc_can_scale_clk(host));
+}
+
+static ssize_t store_enable(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+	unsigned long value, freq;
+	int retval = -EINVAL;
+
+	if (!host)
+		goto out;
+
+	mmc_claim_host(host);
+	if (!host->card || kstrtoul(buf, 0, &value))
+		goto err;
+
+	if (value && !mmc_can_scale_clk(host)) {
+		if (mmc_card_ddr_mode(host->card) ||
+				mmc_card_hs200(host->card) ||
+				mmc_card_uhs(host->card)) {
+			host->caps2 |= MMC_CAP2_CLK_SCALE;
+			mmc_init_clk_scaling(host);
+		}
+
+		if (!mmc_can_scale_clk(host)) {
+			host->caps2 &= ~MMC_CAP2_CLK_SCALE;
+			goto err;
+		}
+	} else if (!value && mmc_can_scale_clk(host)) {
+		host->caps2 &= ~MMC_CAP2_CLK_SCALE;
+		mmc_disable_clk_scaling(host);
+
+		/* Set to max. frequency, since we are disabling */
+		if (host->bus_ops && host->bus_ops->change_bus_speed) {
+			freq = mmc_get_max_frequency(host);
+			if (host->bus_ops->change_bus_speed(host, &freq))
+				goto err;
+		}
+		host->clk_scaling.initialized = false;
+	}
+	retval = count;
+err:
+	mmc_release_host(host);
+out:
+	return retval;
+}
+
+static ssize_t show_up_threshold(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+
+	if (!host)
+		return -EINVAL;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", host->clk_scaling.up_threshold);
+}
+
+#define MAX_PERCENTAGE	100
+static ssize_t store_up_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+	unsigned long value;
+
+	if (!host || kstrtoul(buf, 0, &value) || (value > MAX_PERCENTAGE))
+		return -EINVAL;
+
+	host->clk_scaling.up_threshold = value;
+
+	pr_debug("%s: clkscale_up_thresh set to %lu\n",
+			mmc_hostname(host), value);
+	return count;
+}
+
+static ssize_t show_down_threshold(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+
+	if (!host)
+		return -EINVAL;
+
+	return snprintf(buf, PAGE_SIZE, "%d\n",
+			host->clk_scaling.down_threshold);
+}
+
+static ssize_t store_down_threshold(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+	unsigned long value;
+
+	if (!host || kstrtoul(buf, 0, &value) || (value > MAX_PERCENTAGE))
+		return -EINVAL;
+
+	host->clk_scaling.down_threshold = value;
+
+	pr_debug("%s: clkscale_down_thresh set to %lu\n",
+			mmc_hostname(host), value);
+	return count;
+}
+
+static ssize_t show_polling(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+
+	if (!host)
+		return -EINVAL;
+
+	return snprintf(buf, PAGE_SIZE, "%lu milliseconds\n",
+			host->clk_scaling.polling_delay_ms);
+}
+
+static ssize_t store_polling(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct mmc_host *host = cls_dev_to_mmc_host(dev);
+	unsigned long value;
+
+	if (!host || kstrtoul(buf, 0, &value))
+		return -EINVAL;
+
+	host->clk_scaling.polling_delay_ms = value;
+
+	pr_debug("%s: clkscale_polling_delay_ms set to %lu\n",
+			mmc_hostname(host), value);
+	return count;
+}
+
+DEVICE_ATTR(enable, S_IRUGO | S_IWUSR,
+		show_enable, store_enable);
+DEVICE_ATTR(polling_interval, S_IRUGO | S_IWUSR,
+		show_polling, store_polling);
+DEVICE_ATTR(up_threshold, S_IRUGO | S_IWUSR,
+		show_up_threshold, store_up_threshold);
+DEVICE_ATTR(down_threshold, S_IRUGO | S_IWUSR,
+		show_down_threshold, store_down_threshold);
+
+static struct attribute *clk_scaling_attrs[] = {
+	&dev_attr_enable.attr,
+	&dev_attr_up_threshold.attr,
+	&dev_attr_down_threshold.attr,
+	&dev_attr_polling_interval.attr,
+	NULL,
+};
+
+static struct attribute_group clk_scaling_attr_grp = {
+	.name = "clk_scaling",
+	.attrs = clk_scaling_attrs,
+};
+
 #ifdef CONFIG_MMC_PERF_PROFILING
 static ssize_t
 show_perf(struct device *dev, struct device_attribute *attr, char *buf)
@@ -453,6 +617,11 @@
 	host->clk_scaling.down_threshold = 5;
 	host->clk_scaling.polling_delay_ms = 100;
 
+	err = sysfs_create_group(&host->class_dev.kobj, &clk_scaling_attr_grp);
+	if (err)
+		pr_err("%s: failed to create clk scale sysfs group with err %d\n",
+				__func__, err);
+
 	err = sysfs_create_group(&host->parent->kobj, &dev_attr_grp);
 	if (err)
 		pr_err("%s: failed to create sysfs group with err %d\n",
@@ -486,7 +655,7 @@
 	mmc_remove_host_debugfs(host);
 #endif
 	sysfs_remove_group(&host->parent->kobj, &dev_attr_grp);
-
+	sysfs_remove_group(&host->class_dev.kobj, &clk_scaling_attr_grp);
 
 	device_del(&host->class_dev);
 
diff --git a/drivers/mmc/host/msm_sdcc.c b/drivers/mmc/host/msm_sdcc.c
index c8b47b9..e8f3d38 100644
--- a/drivers/mmc/host/msm_sdcc.c
+++ b/drivers/mmc/host/msm_sdcc.c
@@ -1640,6 +1640,11 @@
 
 	spin_lock(&host->lock);
 
+	if (!atomic_read(&host->clks_on)) {
+		spin_unlock(&host->lock);
+		return IRQ_NONE;
+	}
+
 	status = readl_relaxed(base + MMCISTATUS);
 
 	if (((readl_relaxed(host->base + MMCIMASK0) & status) &
@@ -2177,13 +2182,7 @@
 	spin_lock_irqsave(&host->lock, flags);
 
 	if (host->eject) {
-		if (mrq->data && !(mrq->data->flags & MMC_DATA_READ)) {
-			mrq->cmd->error = 0;
-			mrq->data->bytes_xfered = mrq->data->blksz *
-						  mrq->data->blocks;
-		} else
-			mrq->cmd->error = -ENOMEDIUM;
-
+		mrq->cmd->error = -ENOMEDIUM;
 		spin_unlock_irqrestore(&host->lock, flags);
 		mmc_request_done(mmc, mrq);
 		return;
@@ -5104,6 +5103,31 @@
 	}
 }
 
+/*
+ * This function prints the testbus debug output for all the
+ * available SDCC controller test bus.
+ *
+ * Note: This function should only be called if the SDCC is clocked.
+ */
+static void msmsdcc_print_testbus_info(struct msmsdcc_host *host)
+{
+	int testbus_num;
+
+	if (!is_testbus_debug(host))
+		return;
+
+	pr_err("== SDCC Test Bus Debug ==");
+	for (testbus_num = 0; testbus_num < MAX_TESTBUS; testbus_num++) {
+		writel_relaxed(((testbus_num & MCI_TESTBUS_SEL_MASK)
+			       | MCI_TESTBUS_ENA),
+			       host->base + MCI_TESTBUS_CONFIG);
+		pr_err("TestBus(%d) = 0x%.8x\n", testbus_num,
+			(u32)readl_relaxed(host->base + MCI_SDCC_DEBUG_REG));
+	}
+	/* Disable the test bus output */
+	writel_relaxed(~MCI_TESTBUS_ENA, host->base + MCI_TESTBUS_CONFIG);
+}
+
 static void msmsdcc_dump_sdcc_state(struct msmsdcc_host *host)
 {
 	/* Dump current state of SDCC clocks, power and irq */
@@ -5123,6 +5147,7 @@
 		pr_err("%s: MCI_TEST_INPUT = 0x%.8x\n",
 			mmc_hostname(host->mmc),
 			readl_relaxed(host->base + MCI_TEST_INPUT));
+		msmsdcc_print_testbus_info(host);
 	}
 
 	if (host->curr.data) {
@@ -6536,19 +6561,19 @@
 
 #if CONFIG_DEBUG_FS
 static void msmsdcc_print_pm_stats(struct msmsdcc_host *host, ktime_t start,
-		const char *func)
+				   const char *func, int err)
 {
 	ktime_t diff;
 
-	if (host->print_pm_stats) {
+	if (host->print_pm_stats && !err) {
 		diff = ktime_sub(ktime_get(), start);
-		pr_info("%s: %s: Completed in %llu usec\n", func,
-		mmc_hostname(host->mmc), (u64)ktime_to_us(diff));
+		pr_info("%s: %s: Completed in %llu usec\n",
+			mmc_hostname(host->mmc), func, (u64)ktime_to_us(diff));
 	}
 }
 #else
 static void msmsdcc_print_pm_stats(struct msmsdcc_host *host, ktime_t start,
-		const char *func) {}
+				   const char *func, int err) {}
 #endif
 
 static int
@@ -6615,7 +6640,7 @@
 out:
 	/* set bus bandwidth to 0 immediately */
 	msmsdcc_msm_bus_cancel_work_and_set_vote(host, NULL);
-	msmsdcc_print_pm_stats(host, start, __func__);
+	msmsdcc_print_pm_stats(host, start, __func__, rc);
 	return rc;
 }
 
@@ -6664,7 +6689,7 @@
 	host->pending_resume = false;
 	pr_debug("%s: %s: end\n", mmc_hostname(mmc), __func__);
 out:
-	msmsdcc_print_pm_stats(host, start, __func__);
+	msmsdcc_print_pm_stats(host, start, __func__, 0);
 	return 0;
 }
 
@@ -6701,7 +6726,7 @@
 	if (!pm_runtime_suspended(dev))
 		rc = msmsdcc_runtime_suspend(dev);
  out:
-	msmsdcc_print_pm_stats(host, start, __func__);
+	msmsdcc_print_pm_stats(host, start, __func__, rc);
 	return rc;
 }
 
@@ -6757,7 +6782,7 @@
 		enable_irq(host->plat->status_irq);
 	}
 out:
-	msmsdcc_print_pm_stats(host, start, __func__);
+	msmsdcc_print_pm_stats(host, start, __func__, rc);
 	return rc;
 }
 
diff --git a/drivers/mmc/host/msm_sdcc.h b/drivers/mmc/host/msm_sdcc.h
index 500b5fb..7469c8e 100644
--- a/drivers/mmc/host/msm_sdcc.h
+++ b/drivers/mmc/host/msm_sdcc.h
@@ -193,6 +193,13 @@
 
 #define MCI_TEST_INPUT		0x0D4
 
+#define MCI_TESTBUS_CONFIG	0x0CC
+#define MCI_TESTBUS_SEL_MASK	(0x7)
+#define MAX_TESTBUS		8
+#define MCI_TESTBUS_ENA		(1 << 3)
+
+#define MCI_SDCC_DEBUG_REG	0x124
+
 #define MCI_IRQENABLE	\
 	(MCI_CMDCRCFAILMASK|MCI_DATACRCFAILMASK|MCI_CMDTIMEOUTMASK|	\
 	MCI_DATATIMEOUTMASK|MCI_TXUNDERRUNMASK|MCI_RXOVERRUNMASK|	\
@@ -449,6 +456,7 @@
 #define MSMSDCC_AUTO_CMD21	(1 << 10)
 #define MSMSDCC_SW_RST_CFG_BROKEN	(1 << 11)
 #define MSMSDCC_DATA_PEND_FOR_CMD53	(1 << 12)
+#define MSMSDCC_TESTBUS_DEBUG		(1 << 13)
 
 #define set_hw_caps(h, val)		((h)->hw_caps |= val)
 #define is_sps_mode(h)			((h)->hw_caps & MSMSDCC_SPS_BAM_SUP)
@@ -465,6 +473,7 @@
 #define is_sw_reset_save_config_broken(h) \
 				((h)->hw_caps & MSMSDCC_SW_RST_CFG_BROKEN)
 #define is_data_pend_for_cmd53(h) ((h)->hw_caps & MSMSDCC_DATA_PEND_FOR_CMD53)
+#define is_testbus_debug(h) ((h)->hw_caps & MSMSDCC_TESTBUS_DEBUG)
 
 /* Set controller capabilities based on version */
 static inline void set_default_hw_caps(struct msmsdcc_host *host)
@@ -498,7 +507,8 @@
 	if (step >= 0x2b) /* SDCC v4 2.1.0 and greater */
 		host->hw_caps |= MSMSDCC_SW_RST | MSMSDCC_SW_RST_CFG |
 				 MSMSDCC_AUTO_CMD21 |
-				 MSMSDCC_DATA_PEND_FOR_CMD53;
+				 MSMSDCC_DATA_PEND_FOR_CMD53 |
+				 MSMSDCC_TESTBUS_DEBUG;
 
 	if (step == 0x2b)
 		host->hw_caps |= MSMSDCC_SW_RST_CFG_BROKEN;
diff --git a/drivers/net/usb/rmnet_usb_ctrl.c b/drivers/net/usb/rmnet_usb_ctrl.c
index f87b3b9..1476bb3 100644
--- a/drivers/net/usb/rmnet_usb_ctrl.c
+++ b/drivers/net/usb/rmnet_usb_ctrl.c
@@ -24,6 +24,7 @@
 #define DEVICE_NAME			"hsicctl"
 #define NUM_CTRL_CHANNELS		4
 #define DEFAULT_READ_URB_LENGTH		0x1000
+#define UNLINK_TIMEOUT_MS		500 /*random value*/
 
 /*Output control lines.*/
 #define ACM_CTRL_DTR		BIT(0)
@@ -535,6 +536,7 @@
 	struct ctrl_pkt_list_elem	*list_elem = NULL;
 	struct rmnet_ctrl_dev		*dev;
 	unsigned long			flag;
+	int				time;
 
 	dev = file->private_data;
 	if (!dev)
@@ -558,7 +560,9 @@
 	dev->is_opened = 0;
 	mutex_unlock(&dev->dev_lock);
 
-	if (is_dev_connected(dev))
+	time = usb_wait_anchor_empty_timeout(&dev->tx_submitted,
+			UNLINK_TIMEOUT_MS);
+	if (!time)
 		usb_kill_anchored_urbs(&dev->tx_submitted);
 
 	file->private_data = NULL;
diff --git a/drivers/of/base.c b/drivers/of/base.c
index 5806449..715aef2 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -24,32 +24,15 @@
 #include <linux/slab.h>
 #include <linux/proc_fs.h>
 
-/**
- * struct alias_prop - Alias property in 'aliases' node
- * @link:	List node to link the structure in aliases_lookup list
- * @alias:	Alias property name
- * @np:		Pointer to device_node that the alias stands for
- * @id:		Index value from end of alias name
- * @stem:	Alias string without the index
- *
- * The structure represents one alias property of 'aliases' node as
- * an entry in aliases_lookup list.
- */
-struct alias_prop {
-	struct list_head link;
-	const char *alias;
-	struct device_node *np;
-	int id;
-	char stem[0];
-};
+#include "of_private.h"
 
-static LIST_HEAD(aliases_lookup);
+LIST_HEAD(aliases_lookup);
 
 struct device_node *allnodes;
 struct device_node *of_chosen;
 struct device_node *of_aliases;
 
-static DEFINE_MUTEX(of_aliases_mutex);
+DEFINE_MUTEX(of_aliases_mutex);
 
 /* use when traversing tree through the allnext, child, sibling,
  * or parent members of struct device_node.
diff --git a/drivers/of/device.c b/drivers/of/device.c
index 4c74e4f..b8d31db 100644
--- a/drivers/of/device.c
+++ b/drivers/of/device.c
@@ -8,6 +8,7 @@
 #include <linux/slab.h>
 
 #include <asm/errno.h>
+#include "of_private.h"
 
 /**
  * of_match_device - Tell if a struct device matches an of_device_id list
@@ -131,6 +132,7 @@
 void of_device_uevent(struct device *dev, struct kobj_uevent_env *env)
 {
 	const char *compat;
+	struct alias_prop *app;
 	int seen = 0, cplen, sl;
 
 	if ((!dev) || (!dev->of_node))
@@ -153,6 +155,21 @@
 		seen++;
 	}
 	add_uevent_var(env, "OF_COMPATIBLE_N=%d", seen);
+
+	seen = 0;
+	mutex_lock(&of_aliases_mutex);
+	list_for_each_entry(app, &aliases_lookup, link) {
+		if (dev->of_node == app->np) {
+			add_uevent_var(env, "OF_ALIAS_%d=%s", seen,
+				       app->alias);
+			seen++;
+		}
+	}
+
+	if (seen)
+		add_uevent_var(env, "OF_ALIAS_N=%d", seen);
+
+	mutex_unlock(&of_aliases_mutex);
 }
 
 int of_device_uevent_modalias(struct device *dev, struct kobj_uevent_env *env)
diff --git a/drivers/of/of_private.h b/drivers/of/of_private.h
new file mode 100644
index 0000000..b38415c
--- /dev/null
+++ b/drivers/of/of_private.h
@@ -0,0 +1,41 @@
+/*
+ * Procedures for creating, accessing and interpreting the device tree.
+ *
+ * Paul Mackerras	August 1996.
+ * Copyright (C) 1996-2005 Paul Mackerras.
+ *
+ *  Adapted for 64bit PowerPC by Dave Engebretsen and Peter Bergner.
+ *    {engebret|bergner}@us.ibm.com
+ *
+ *  Adapted for sparc and sparc64 by David S. Miller davem@davemloft.net
+ *
+ *  Reconsolidated from arch/x/kernel/prom.c by Stephen Rothwell and
+ *  Grant Likely.
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License
+ *      as published by the Free Software Foundation; either version
+ *      2 of the License, or (at your option) any later version.
+ */
+
+/**
+ * struct alias_prop - Alias property in 'aliases' node
+ * @link:	List node to link the structure in aliases_lookup list
+ * @alias:	Alias property name
+ * @np:		Pointer to device_node that the alias stands for
+ * @id:		Index value from end of alias name
+ * @stem:	Alias string without the index
+ *
+ * The structure represents one alias property of 'aliases' node as
+ * an entry in aliases_lookup list.
+ */
+struct alias_prop {
+	struct list_head link;
+	const char *alias;
+	struct device_node *np;
+	int id;
+	char stem[0];
+};
+
+extern struct mutex of_aliases_mutex;
+extern struct list_head aliases_lookup;
diff --git a/drivers/platform/msm/Kconfig b/drivers/platform/msm/Kconfig
index 872a9b5..9394986 100644
--- a/drivers/platform/msm/Kconfig
+++ b/drivers/platform/msm/Kconfig
@@ -85,5 +85,11 @@
 	  for the IPA core.
 	  Kernel and user-space processes can call the IPA driver
 	  to configure IPA core.
+config MSM_AVTIMER
+	tristate "Avtimer Driver"
+	depends on ARCH_MSM8960
+	help
+		This driver gets the Q6 out of power collapsed state and
+		exposes ioctl control to read avtimer tick.
 
 endmenu
diff --git a/drivers/platform/msm/Makefile b/drivers/platform/msm/Makefile
index 0a755d3..919c07f 100644
--- a/drivers/platform/msm/Makefile
+++ b/drivers/platform/msm/Makefile
@@ -8,3 +8,4 @@
 obj-$(CONFIG_QPNP_PWM) += qpnp-pwm.o
 obj-$(CONFIG_QPNP_POWER_ON) += qpnp-power-on.o
 obj-$(CONFIG_QPNP_CLKDIV) += qpnp-clkdiv.o
+obj-$(CONFIG_MSM_AVTIMER) += avtimer.o
diff --git a/drivers/platform/msm/avtimer.c b/drivers/platform/msm/avtimer.c
new file mode 100644
index 0000000..f513ceb
--- /dev/null
+++ b/drivers/platform/msm/avtimer.c
@@ -0,0 +1,369 @@
+
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+
+* This program is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 and
+* only version 2 as published by the Free Software Foundation.
+
+* This program is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+*/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/uaccess.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/avtimer.h>
+#include <mach/qdsp6v2/apr.h>
+
+#define DEVICE_NAME "avtimer"
+
+
+#define ADSP_CMD_SET_POWER_COLLAPSE_STATE 0x0001115C
+
+static int major;	/* Major number assigned to our device driver */
+struct avtimer_t {
+	struct cdev myc;
+	struct class *avtimer_class;
+	struct mutex avtimer_lock;
+	int avtimer_open_cnt;
+	struct dev_avtimer_data *avtimer_pdata;
+};
+static struct avtimer_t avtimer;
+
+static struct apr_svc *core_handle;
+
+struct adsp_power_collapse {
+	struct apr_hdr hdr;
+	uint32_t power_collapse;
+};
+
+static int32_t avcs_core_callback(struct apr_client_data *data, void *priv)
+{
+	uint32_t *payload;
+
+	pr_debug("core msg: payload len = %u, apr resp opcode = 0x%X\n",
+		data->payload_size, data->opcode);
+
+	switch (data->opcode) {
+
+	case APR_BASIC_RSP_RESULT:{
+
+		if (data->payload_size == 0) {
+			pr_err("%s: APR_BASIC_RSP_RESULT No Payload ",
+					__func__);
+			return 0;
+		}
+
+		payload = data->payload;
+
+		switch (payload[0]) {
+
+		case ADSP_CMD_SET_POWER_COLLAPSE_STATE:
+			pr_debug("CMD_SET_POWER_COLLAPSE_STATE status[0x%x]\n",
+					payload[1]);
+			break;
+		default:
+			pr_err("Invalid cmd rsp[0x%x][0x%x]\n",
+					payload[0], payload[1]);
+			break;
+		}
+		break;
+	}
+	case RESET_EVENTS:{
+		pr_debug("Reset event received in Core service");
+		apr_reset(core_handle);
+		core_handle = NULL;
+		break;
+	}
+
+	default:
+		pr_err("Message id from adsp core svc: %d\n", data->opcode);
+		break;
+	}
+
+	return 0;
+}
+
+int avcs_core_open(void)
+{
+	if (core_handle == NULL)
+		core_handle = apr_register("ADSP", "CORE",
+					avcs_core_callback, 0xFFFFFFFF, NULL);
+
+	pr_debug("Open_q %p\n", core_handle);
+	if (core_handle == NULL) {
+		pr_err("%s: Unable to register CORE\n", __func__);
+		return -ENODEV;
+	}
+	return 0;
+}
+
+int avcs_core_disable_power_collapse(int disable)
+{
+	struct adsp_power_collapse pc;
+	int rc = 0;
+
+	if (core_handle) {
+		pc.hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_EVENT,
+			APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+		pc.hdr.pkt_size = APR_PKT_SIZE(APR_HDR_SIZE,
+					sizeof(uint32_t));
+		pc.hdr.src_port = 0;
+		pc.hdr.dest_port = 0;
+		pc.hdr.token = 0;
+		pc.hdr.opcode = ADSP_CMD_SET_POWER_COLLAPSE_STATE;
+		/*
+		* When power_collapse set to 1 -- If the aDSP is in the power
+		* collapsed state when this command is received, it is awakened
+		* from this state. The aDSP does not power collapse again until
+		* the client revokes this	command
+		* When power_collapse set to 0 -- This indicates to the aDSP
+		* that the remote client does not need it to be out of power
+		* collapse any longer. This may not always put the aDSP into
+		* power collapse; the aDSP must honor an internal client's
+		* power requirements as well.
+		*/
+		pc.power_collapse = disable;
+		rc = apr_send_pkt(core_handle, (uint32_t *)&pc);
+		if (rc < 0) {
+			pr_debug("disable power collapse = %d failed\n",
+				disable);
+			return rc;
+		}
+		pr_debug("disable power collapse = %d\n", disable);
+	}
+	return 0;
+}
+
+static int avtimer_open(struct inode *inode, struct file *file)
+{
+	int rc = 0;
+	struct avtimer_t *pavtimer = &avtimer;
+
+	pr_debug("avtimer_open\n");
+	mutex_lock(&pavtimer->avtimer_lock);
+
+	if (pavtimer->avtimer_open_cnt != 0) {
+		pavtimer->avtimer_open_cnt++;
+		pr_debug("%s: opened avtimer open count=%d\n",
+			__func__, pavtimer->avtimer_open_cnt);
+		mutex_unlock(&pavtimer->avtimer_lock);
+		return 0;
+	}
+	try_module_get(THIS_MODULE);
+
+	rc = avcs_core_open();
+	if (core_handle)
+		rc = avcs_core_disable_power_collapse(1);
+
+	pavtimer->avtimer_open_cnt++;
+	pr_debug("%s: opened avtimer open count=%d\n",
+		__func__, pavtimer->avtimer_open_cnt);
+	mutex_unlock(&pavtimer->avtimer_lock);
+	pr_debug("avtimer_open leave rc=%d\n", rc);
+
+	return rc;
+}
+
+static int avtimer_release(struct inode *inode, struct file *file)
+{
+	int rc = 0;
+	struct avtimer_t *pavtimer = &avtimer;
+
+	mutex_lock(&pavtimer->avtimer_lock);
+	pavtimer->avtimer_open_cnt--;
+
+	if (core_handle && pavtimer->avtimer_open_cnt == 0)
+		rc = avcs_core_disable_power_collapse(0);
+
+	pr_debug("device_release(%p,%p) open count=%d\n",
+		inode, file, pavtimer->avtimer_open_cnt);
+
+	module_put(THIS_MODULE);
+
+	mutex_unlock(&pavtimer->avtimer_lock);
+
+	return rc;
+}
+
+/*
+ * ioctl call provides GET_AVTIMER
+ */
+static long avtimer_ioctl(struct file *file, unsigned int ioctl_num,
+				unsigned long ioctl_param)
+{
+	struct avtimer_t *pavtimer = &avtimer;
+	pr_debug("avtimer_ioctl: ioctlnum=%d,param=%lx\n",
+				ioctl_num, ioctl_param);
+
+	switch (ioctl_num) {
+	case IOCTL_GET_AVTIMER_TICK:
+	{
+		void __iomem *p_avtimer_msw = NULL, *p_avtimer_lsw = NULL;
+		uint32_t avtimer_msw_1st = 0, avtimer_lsw = 0;
+		uint32_t avtimer_msw_2nd = 0;
+		uint64_t avtimer_tick;
+
+		if (pavtimer->avtimer_pdata) {
+			p_avtimer_lsw = ioremap(
+			pavtimer->avtimer_pdata->avtimer_lsw_phy_addr, 4);
+			p_avtimer_msw = ioremap(
+			pavtimer->avtimer_pdata->avtimer_msw_phy_addr, 4);
+		}
+		if (!p_avtimer_lsw || !p_avtimer_msw) {
+			pr_err("ioremap failed\n");
+			return -EIO;
+		}
+		do {
+			avtimer_msw_1st = ioread32(p_avtimer_msw);
+			avtimer_lsw = ioread32(p_avtimer_lsw);
+			avtimer_msw_2nd = ioread32(p_avtimer_msw);
+		} while (avtimer_msw_1st != avtimer_msw_2nd);
+
+		avtimer_tick =
+		((uint64_t) avtimer_msw_1st << 32) | avtimer_lsw;
+
+		pr_debug("AV Timer tick: msw: %d, lsw: %d\n", avtimer_msw_1st,
+				avtimer_lsw);
+		if (copy_to_user((void *) ioctl_param, &avtimer_tick,
+				sizeof(avtimer_tick))) {
+					pr_err("copy_to_user failed\n");
+					iounmap(p_avtimer_lsw);
+					iounmap(p_avtimer_msw);
+					return -EFAULT;
+			}
+		iounmap(p_avtimer_lsw);
+		iounmap(p_avtimer_msw);
+		}
+		break;
+
+	default:
+		pr_err("invalid cmd\n");
+		break;
+	}
+
+	return 0;
+}
+
+static const struct file_operations avtimer_fops = {
+	.unlocked_ioctl = avtimer_ioctl,
+	.open = avtimer_open,
+	.release = avtimer_release
+};
+
+static int dev_avtimer_probe(struct platform_device *pdev)
+{
+	int result;
+	dev_t dev = MKDEV(major, 0);
+	struct device *device_handle;
+	struct avtimer_t *pavtimer = &avtimer;
+
+	/* get the device number */
+	if (major)
+		result = register_chrdev_region(dev, 1, DEVICE_NAME);
+	else {
+		result = alloc_chrdev_region(&dev, 0, 1, DEVICE_NAME);
+		major = MAJOR(dev);
+	}
+
+	if (result < 0) {
+		pr_err("Registering avtimer device failed\n");
+		return result;
+	}
+
+	pavtimer->avtimer_class = class_create(THIS_MODULE, "avtimer");
+	if (IS_ERR(pavtimer->avtimer_class)) {
+		result = PTR_ERR(pavtimer->avtimer_class);
+		pr_err("Error creating avtimer class: %d\n", result);
+		goto unregister_chrdev_region;
+	}
+	pavtimer->avtimer_pdata = pdev->dev.platform_data;
+
+	cdev_init(&pavtimer->myc, &avtimer_fops);
+	result = cdev_add(&pavtimer->myc, dev, 1);
+
+	if (result < 0) {
+		pr_err("Registering file operations failed\n");
+		goto class_destroy;
+	}
+
+	device_handle = device_create(pavtimer->avtimer_class,
+			NULL, pavtimer->myc.dev, NULL, "avtimer");
+	if (IS_ERR(device_handle)) {
+		result = PTR_ERR(device_handle);
+		pr_err("device_create failed: %d\n", result);
+		goto class_destroy;
+	}
+
+	mutex_init(&pavtimer->avtimer_lock);
+	core_handle = NULL;
+	pavtimer->avtimer_open_cnt = 0;
+
+	pr_debug("Device create done for avtimer major=%d\n", major);
+
+	return 0;
+
+class_destroy:
+	class_destroy(pavtimer->avtimer_class);
+unregister_chrdev_region:
+	unregister_chrdev_region(MKDEV(major, 0), 1);
+	return result;
+
+}
+
+static int __devexit dev_avtimer_remove(struct platform_device *pdev)
+{
+	struct avtimer_t *pavtimer = &avtimer;
+
+	pr_debug("dev_avtimer_remove\n");
+
+	device_destroy(pavtimer->avtimer_class, pavtimer->myc.dev);
+	cdev_del(&pavtimer->myc);
+	class_destroy(pavtimer->avtimer_class);
+	unregister_chrdev_region(MKDEV(major, 0), 1);
+
+	return 0;
+}
+
+static struct platform_driver dev_avtimer_driver = {
+	.probe = dev_avtimer_probe,
+	.remove = __exit_p(dev_avtimer_remove),
+	.driver = {.name = "dev_avtimer"}
+};
+
+static int  __init avtimer_init(void)
+{
+	s32 rc;
+	rc = platform_driver_register(&dev_avtimer_driver);
+	if (IS_ERR_VALUE(rc)) {
+		pr_err("platform_driver_register failed.\n");
+		goto error_platform_driver;
+	}
+	pr_debug("dev_avtimer_init : done\n");
+
+	return 0;
+error_platform_driver:
+
+	pr_err("encounterd error\n");
+	return -ENODEV;
+}
+
+static void __exit avtimer_exit(void)
+{
+	pr_debug("avtimer_exit\n");
+	platform_driver_unregister(&dev_avtimer_driver);
+}
+
+module_init(avtimer_init);
+module_exit(avtimer_exit);
+
+MODULE_DESCRIPTION("avtimer driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/platform/msm/ipa/ipa.c b/drivers/platform/msm/ipa/ipa.c
index 8f68ef5..7973cfe 100644
--- a/drivers/platform/msm/ipa/ipa.c
+++ b/drivers/platform/msm/ipa/ipa.c
@@ -637,7 +637,7 @@
 
 		switch (tx_pkt->cnt) {
 		case 1:
-			ipa_write_done(&tx_pkt->work);
+			ipa_wq_write_done(&tx_pkt->work);
 			break;
 		case 0xFFFF:
 			/* reached end of set */
@@ -651,7 +651,7 @@
 			   list_first_entry(&sys->head_desc_list,
 					    struct ipa_tx_pkt_wrapper, link);
 			spin_unlock_irqrestore(&sys->spinlock, irq_flags);
-			ipa_write_done(&tx_pkt->work);
+			ipa_wq_write_done(&tx_pkt->work);
 			break;
 		default:
 			/* keep looping till reach the end of the set */
diff --git a/drivers/platform/msm/ipa/ipa_bridge.c b/drivers/platform/msm/ipa/ipa_bridge.c
index cf51ab6..a6221b8 100644
--- a/drivers/platform/msm/ipa/ipa_bridge.c
+++ b/drivers/platform/msm/ipa/ipa_bridge.c
@@ -24,7 +24,7 @@
 
 static int polling_min_sleep[IPA_DIR_MAX] = { 950, 950 };
 static int polling_max_sleep[IPA_DIR_MAX] = { 1050, 1050 };
-static int polling_inactivity[IPA_DIR_MAX] = { 20, 20 };
+static int polling_inactivity[IPA_DIR_MAX] = { 4, 4 };
 
 struct ipa_pkt_info {
 	void *buffer;
@@ -167,6 +167,34 @@
 	return -ENOMEM;
 }
 
+static int ipa_reclaim_tx(struct ipa_bridge_pipe_context *sys_tx, bool all)
+{
+	struct sps_iovec iov;
+	struct ipa_pkt_info *tx_pkt;
+	int cnt = 0;
+	int ret;
+
+	do {
+		iov.addr = 0;
+		ret = sps_get_iovec(sys_tx->pipe, &iov);
+		if (ret || iov.addr == 0) {
+			break;
+		} else {
+			tx_pkt = list_first_entry(&sys_tx->head_desc_list,
+						  struct ipa_pkt_info,
+						  list_node);
+			list_move_tail(&tx_pkt->list_node,
+					&sys_tx->free_desc_list);
+			sys_tx->len--;
+			sys_tx->free_len++;
+			tx_pkt->len = ~0;
+			cnt++;
+		}
+	} while (all);
+
+	return cnt;
+}
+
 static void ipa_do_bridge_work(enum ipa_bridge_dir dir)
 {
 	struct ipa_bridge_pipe_context *sys_rx = &bridge[2 * dir];
@@ -180,22 +208,9 @@
 
 	while (1) {
 		++inactive_cycles;
-		iov.addr = 0;
-		ret = sps_get_iovec(sys_tx->pipe, &iov);
-		if (ret || iov.addr == 0) {
-			/* no-op */
-		} else {
-			inactive_cycles = 0;
 
-			tx_pkt = list_first_entry(&sys_tx->head_desc_list,
-						  struct ipa_pkt_info,
-						  list_node);
-			list_move_tail(&tx_pkt->list_node,
-					&sys_tx->free_desc_list);
-			sys_tx->len--;
-			sys_tx->free_len++;
-			tx_pkt->len = ~0;
-		}
+		if (ipa_reclaim_tx(sys_tx, false))
+			inactive_cycles = 0;
 
 		iov.addr = 0;
 		ret = sps_get_iovec(sys_rx->pipe, &iov);
@@ -216,7 +231,7 @@
 				tmp_pkt = kmalloc(sizeof(struct ipa_pkt_info),
 						GFP_KERNEL);
 				if (!tmp_pkt) {
-					pr_err_ratelimited("%s: unable to alloc tx_pkt_info\n",
+					pr_debug_ratelimited("%s: unable to alloc tx_pkt_info\n",
 					       __func__);
 					usleep_range(polling_min_sleep[dir],
 							polling_max_sleep[dir]);
@@ -226,7 +241,7 @@
 				tmp_pkt->buffer = kmalloc(IPA_RX_SKB_SIZE,
 						GFP_KERNEL | GFP_DMA);
 				if (!tmp_pkt->buffer) {
-					pr_err_ratelimited("%s: unable to alloc tx_pkt_buffer\n",
+					pr_debug_ratelimited("%s: unable to alloc tx_pkt_buffer\n",
 					       __func__);
 					kfree(tmp_pkt);
 					usleep_range(polling_min_sleep[dir],
@@ -240,7 +255,7 @@
 						DMA_BIDIRECTIONAL);
 				if (tmp_pkt->dma_address == 0 ||
 						tmp_pkt->dma_address == ~0) {
-					pr_err_ratelimited("%s: dma_map_single failure %p for %p\n",
+					pr_debug_ratelimited("%s: dma_map_single failure %p for %p\n",
 					       __func__,
 					       (void *)tmp_pkt->dma_address,
 					       tmp_pkt->buffer);
@@ -271,7 +286,7 @@
 					SPS_IOVEC_FLAG_EOT);
 			if (ret) {
 				list_del(&tx_pkt->list_node);
-				pr_err_ratelimited("%s: sps_transfer_one failed %d\n",
+				pr_debug_ratelimited("%s: sps_transfer_one failed %d\n",
 						__func__, ret);
 				usleep_range(polling_min_sleep[dir],
 						polling_max_sleep[dir]);
@@ -289,9 +304,10 @@
 					       SPS_IOVEC_FLAG_INT |
 					       SPS_IOVEC_FLAG_EOT);
 			if (ret) {
-				pr_err_ratelimited("%s: fail to add to TX dir=%d\n",
+				pr_debug_ratelimited("%s: fail to add to TX dir=%d\n",
 						__func__, dir);
 				list_del(&rx_pkt->list_node);
+				ipa_reclaim_tx(sys_tx, true);
 				usleep_range(polling_min_sleep[dir],
 						polling_max_sleep[dir]);
 				goto retry_add_tx;
@@ -306,7 +322,7 @@
 	}
 }
 
-static void ipa_rx_notify(struct sps_event_notify *notify)
+static void ipa_sps_irq_rx_notify(struct sps_event_notify *notify)
 {
 	switch (notify->event_id) {
 	case SPS_EVENT_EOT:
@@ -457,7 +473,7 @@
 		sys->register_event.options = SPS_O_EOT;
 		sys->register_event.mode = SPS_TRIGGER_CALLBACK;
 		sys->register_event.xfer_done = NULL;
-		sys->register_event.callback = ipa_rx_notify;
+		sys->register_event.callback = ipa_sps_irq_rx_notify;
 		sys->register_event.user = NULL;
 		ret = sps_register_event(sys->pipe, &sys->register_event);
 		if (ret < 0) {
diff --git a/drivers/platform/msm/ipa/ipa_client.c b/drivers/platform/msm/ipa/ipa_client.c
index 823b17d..dc9da7d 100644
--- a/drivers/platform/msm/ipa/ipa_client.c
+++ b/drivers/platform/msm/ipa/ipa_client.c
@@ -157,7 +157,7 @@
 
 	ep->valid = 1;
 	ep->client = in->client;
-	ep->notify = in->notify;
+	ep->client_notify = in->notify;
 	ep->priv = in->priv;
 
 	if (ipa_cfg_ep(ipa_ep_idx, &in->ipa_ep_cfg)) {
diff --git a/drivers/platform/msm/ipa/ipa_dp.c b/drivers/platform/msm/ipa/ipa_dp.c
index c677a6e..4de19d2 100644
--- a/drivers/platform/msm/ipa/ipa_dp.c
+++ b/drivers/platform/msm/ipa/ipa_dp.c
@@ -18,12 +18,21 @@
 
 #define list_next_entry(pos, member) \
 	list_entry(pos->member.next, typeof(*pos), member)
+#define IPA_LAST_DESC_COOKIE 0xFFFF
 /**
- * ipa_write_done - this function will be (enevtually) called when a Tx
+ * ipa_write_done() - this function will be (eventually) called when a Tx
  * operation is complete
- * @work:	work_struct used by the work queue
+ * * @work:	work_struct used by the work queue
+ *
+ * Will be called in deferred context.
+ * - invoke the callback supplied by the client who sent this command
+ * - iterate over all packets and validate that
+ *   the order for sent packet is the same as expected
+ * - delete all the tx packet descriptors from the system
+ *   pipe context (not needed anymore)
+ * - return the tx buffer back to one_kb_no_straddle_pool
  */
-void ipa_write_done(struct work_struct *work)
+void ipa_wq_write_done(struct work_struct *work)
 {
 	struct ipa_tx_pkt_wrapper *tx_pkt;
 	struct ipa_tx_pkt_wrapper *next_pkt;
@@ -40,7 +49,7 @@
 	if (unlikely(cnt == 0))
 		WARN_ON(1);
 
-	if (cnt > 1 && cnt != 0xFFFF)
+	if (cnt > 1 && cnt != IPA_LAST_DESC_COOKIE)
 		mult = tx_pkt->mult;
 
 	for (i = 0; i < cnt; i++) {
@@ -77,6 +86,14 @@
  * @sys:	system pipe context
  * @desc:	descriptor to send
  *
+ * - Allocate tx_packet wrapper
+ * - Allocate a bounce buffer due to HW constrains
+ *   (This buffer will be used for the DMA command)
+ * - Copy the data (desc->pyld) to the bounce buffer
+ * - transfer data to the IPA
+ * - after the transfer was done the SPS will
+ *   notify the sending user via ipa_sps_irq_comp_tx()
+ *
  * Return codes: 0: success, -EFAULT: failure
  */
 int ipa_send_one(struct ipa_sys_context *sys, struct ipa_desc *desc)
@@ -117,7 +134,7 @@
 	}
 
 	INIT_LIST_HEAD(&tx_pkt->link);
-	INIT_WORK(&tx_pkt->work, ipa_write_done);
+	INIT_WORK(&tx_pkt->work, ipa_wq_write_done);
 	tx_pkt->type = desc->type;
 	tx_pkt->cnt = 1;    /* only 1 desc in this "set" */
 
@@ -175,7 +192,20 @@
  * ipa_send() - Send multiple descriptors in one HW transaction
  * @sys: system pipe context
  * @num_desc: number of packets
- * @desc: packets to send
+ * @desc: packets to send (may be immediate command or data)
+ *
+ * This function is used for system-to-bam connection.
+ * - SPS driver expect struct sps_transfer which will contain all the data
+ *   for a transaction
+ * - The sps_transfer struct will be pointing to bounce buffers for
+ *   its DMA command (immediate command and data)
+ * - ipa_tx_pkt_wrapper will be used for each ipa
+ *   descriptor (allocated from wrappers cache)
+ * - The wrapper struct will be configured for each ipa-desc payload and will
+ *   contain information which will be later used by the user callbacks
+ * - each transfer will be made by calling to sps_transfer()
+ * - Each packet (command or data) that will be sent will also be saved in
+ *   ipa_sys_context for later check that all data was sent
  *
  * Return codes: 0: success, -EFAULT: failure
  */
@@ -187,12 +217,20 @@
 	struct sps_iovec *iovec;
 	unsigned long irq_flags;
 	dma_addr_t dma_addr;
-	int i;
+	int i = 0;
 	int j;
 	int result;
-	int fail_dma_wrap;
+	int fail_dma_wrap = 0;
 	uint size = num_desc * sizeof(struct sps_iovec);
 
+	transfer.iovec = dma_alloc_coherent(NULL, size, &dma_addr, 0);
+	transfer.iovec_phys = dma_addr;
+	transfer.iovec_count = num_desc;
+	if (!transfer.iovec) {
+		IPAERR("fail to alloc DMA mem for sps xfr buff\n");
+		goto failure;
+	}
+
 	for (i = 0; i < num_desc; i++) {
 		fail_dma_wrap = 0;
 		tx_pkt = kmem_cache_zalloc(ipa_ctx->tx_pkt_wrapper_cache,
@@ -207,14 +245,6 @@
 		 */
 		if (i == 0) {
 			transfer.user = tx_pkt;
-			transfer.iovec =
-				dma_alloc_coherent(NULL, size, &dma_addr, 0);
-			transfer.iovec_phys = dma_addr;
-			transfer.iovec_count = num_desc;
-			if (!transfer.iovec) {
-				IPAERR("fail alloc DMA mem for sps xfr buff\n");
-				goto failure;
-			}
 
 			tx_pkt->mult.phys_base = dma_addr;
 			tx_pkt->mult.base = transfer.iovec;
@@ -226,7 +256,7 @@
 		iovec->flags = 0;
 
 		INIT_LIST_HEAD(&tx_pkt->link);
-		INIT_WORK(&tx_pkt->work, ipa_write_done);
+		INIT_WORK(&tx_pkt->work, ipa_wq_write_done);
 		tx_pkt->type = desc[i].type;
 
 		tx_pkt->mem.base = desc[i].pyld;
@@ -263,6 +293,10 @@
 		tx_pkt->user1 = desc[i].user1;
 		tx_pkt->user2 = desc[i].user2;
 
+		/*
+		 * Point the iovec to the bounce buffer and
+		 * add this packet to system pipe context.
+		 */
 		iovec->addr = tx_pkt->mem.phys_base;
 		spin_lock_irqsave(&sys->spinlock, irq_flags);
 		list_add_tail(&tx_pkt->link, &sys->head_desc_list);
@@ -284,7 +318,7 @@
 			iovec->flags |= (SPS_IOVEC_FLAG_EOT |
 					SPS_IOVEC_FLAG_INT);
 			/* "mark" the last desc */
-			tx_pkt->cnt = 0xFFFF;
+			tx_pkt->cnt = IPA_LAST_DESC_COOKIE;
 		}
 	}
 
@@ -320,7 +354,7 @@
 }
 
 /**
- * ipa_cmd_ack - callback function which will be called by SPS driver after an
+ * ipa_sps_irq_cmd_ack - callback function which will be called by SPS driver after an
  * immediate command is complete.
  * @user1:	pointer to the descriptor of the transfer
  * @user2:
@@ -328,7 +362,7 @@
  * Complete the immediate commands completion object, this will release the
  * thread which waits on this completion object (ipa_send_cmd())
  */
-static void ipa_cmd_ack(void *user1, void *user2)
+static void ipa_sps_irq_cmd_ack(void *user1, void *user2)
 {
 	struct ipa_desc *desc = (struct ipa_desc *)user1;
 
@@ -340,11 +374,13 @@
 
 /**
  * ipa_send_cmd - send immediate commands
- * @num_desc:	number of descriptors within the descr struct
+ * @num_desc:	number of descriptors within the desc struct
  * @descr:	descriptor structure
  *
  * Function will block till command gets ACK from IPA HW, caller needs
  * to free any resources it allocated after function returns
+ * The callback in ipa_desc should not be set by the caller
+ * for this function.
  */
 int ipa_send_cmd(u16 num_desc, struct ipa_desc *descr)
 {
@@ -353,11 +389,10 @@
 	if (num_desc == 1) {
 		init_completion(&descr->xfer_done);
 
-		/* client should not set these */
 		if (descr->callback || descr->user1)
 			WARN_ON(1);
 
-		descr->callback = ipa_cmd_ack;
+		descr->callback = ipa_sps_irq_cmd_ack;
 		descr->user1 = descr;
 		if (ipa_send_one(&ipa_ctx->sys[IPA_A5_CMD], descr)) {
 			IPAERR("fail to send immediate command\n");
@@ -368,11 +403,10 @@
 		desc = &descr[num_desc - 1];
 		init_completion(&desc->xfer_done);
 
-		/* client should not set these */
 		if (desc->callback || desc->user1)
 			WARN_ON(1);
 
-		desc->callback = ipa_cmd_ack;
+		desc->callback = ipa_sps_irq_cmd_ack;
 		desc->user1 = desc;
 		if (ipa_send(&ipa_ctx->sys[IPA_A5_CMD], num_desc, descr)) {
 			IPAERR("fail to send multiple immediate command set\n");
@@ -385,11 +419,15 @@
 }
 
 /**
- * ipa_tx_notify() - Callback function which will be called by the SPS driver
- * after a Tx operation is complete. Called in an interrupt context.
+ * ipa_sps_irq_tx_notify() - Callback function which will be called by
+ * the SPS driver after a Tx operation is complete.
+ * Called in an interrupt context.
  * @notify:	SPS driver supplied notification struct
+ *
+ * This function defer the work for this event to the tx workqueue.
+ * This event will be later handled by ipa_write_done.
  */
-static void ipa_tx_notify(struct sps_event_notify *notify)
+static void ipa_sps_irq_tx_notify(struct sps_event_notify *notify)
 {
 	struct ipa_tx_pkt_wrapper *tx_pkt;
 
@@ -473,19 +511,19 @@
 		mux_hdr = (struct ipa_a5_mux_hdr *)rx_skb->data;
 
 		IPADBG("RX pkt len=%d IID=0x%x src=%d, flags=0x%x, meta=0x%x\n",
-		       rx_skb->len, ntohs(mux_hdr->interface_id),
-		       mux_hdr->src_pipe_index,
-		       mux_hdr->flags, ntohl(mux_hdr->metadata));
+			rx_skb->len, ntohs(mux_hdr->interface_id),
+			mux_hdr->src_pipe_index,
+			mux_hdr->flags, ntohl(mux_hdr->metadata));
 
 		IPA_DUMP_BUFF(rx_skb->data, 0, rx_skb->len);
 
 		if (mux_hdr->src_pipe_index >= IPA_NUM_PIPES ||
-		    !ipa_ctx->ep[mux_hdr->src_pipe_index].valid ||
-		    !ipa_ctx->ep[mux_hdr->src_pipe_index].notify) {
-			IPAERR("drop pipe=%d ep_valid=%d notify=%p\n",
-			       mux_hdr->src_pipe_index,
-			       ipa_ctx->ep[mux_hdr->src_pipe_index].valid,
-			       ipa_ctx->ep[mux_hdr->src_pipe_index].notify);
+			!ipa_ctx->ep[mux_hdr->src_pipe_index].valid ||
+			!ipa_ctx->ep[mux_hdr->src_pipe_index].client_notify) {
+			IPAERR("drop pipe=%d ep_valid=%d client_notify=%p\n",
+			  mux_hdr->src_pipe_index,
+			  ipa_ctx->ep[mux_hdr->src_pipe_index].valid,
+			  ipa_ctx->ep[mux_hdr->src_pipe_index].client_notify);
 			dev_kfree_skb_any(rx_skb);
 			ipa_replenish_rx_cache();
 			continue;
@@ -505,7 +543,8 @@
 
 		IPADBG("pulling %d bytes from skb\n", pull_len);
 		skb_pull(rx_skb, pull_len);
-		ep->notify(ep->priv, IPA_RECEIVE, (unsigned long)(rx_skb));
+		ep->client_notify(ep->priv, IPA_RECEIVE,
+				(unsigned long)(rx_skb));
 		ipa_replenish_rx_cache();
 	} while (1);
 }
@@ -587,7 +626,7 @@
  * This comes to prevent the CPU from handling too many interrupts when the
  * throughput is high.
  */
-static void ipa_rx_notify(struct sps_event_notify *notify)
+static void ipa_sps_irq_rx_notify(struct sps_event_notify *notify)
 {
 	struct ipa_rx_pkt_wrapper *rx_pkt;
 
@@ -609,9 +648,17 @@
 /**
  * ipa_setup_sys_pipe() - Setup an IPA end-point in system-BAM mode and perform
  * IPA EP configuration
- * @sys_in:	[in] input needed to setup BAM pipe and config EP
+ * @sys_in:	[in] input needed to setup BAM pipe and configure EP
  * @clnt_hdl:	[out] client handle
  *
+ *  - configure the end-point registers with the supplied
+ *    parameters from the user.
+ *  - call SPS APIs to create a system-to-bam connection with IPA.
+ *  - allocate descriptor FIFO
+ *  - register callback function(ipa_sps_irq_rx_notify or
+ *    ipa_sps_irq_tx_notify - depends on client type) in case the driver is
+ *    not configured to pulling mode
+ *
  * Returns:	0 on success, negative on failure
  */
 int ipa_setup_sys_pipe(struct ipa_sys_connect_params *sys_in, u32 *clnt_hdl)
@@ -729,34 +776,21 @@
 	}
 
 	if (!ipa_ctx->polling_mode) {
-		if (IPA_CLIENT_IS_CONS(sys_in->client)) {
-			ipa_ctx->sys[sys_idx].event.options = SPS_O_EOT;
-			ipa_ctx->sys[sys_idx].event.mode = SPS_TRIGGER_CALLBACK;
-			ipa_ctx->sys[sys_idx].event.xfer_done = NULL;
-			ipa_ctx->sys[sys_idx].event.callback = ipa_rx_notify;
-			ipa_ctx->sys[sys_idx].event.user =
-				&ipa_ctx->sys[sys_idx];
-			result =
-			   sps_register_event(ipa_ctx->ep[ipa_ep_idx].ep_hdl,
-					      &ipa_ctx->sys[sys_idx].event);
-			if (result < 0) {
-				IPAERR("rx register event error %d\n", result);
-				goto fail_register_event;
-			}
-		} else {
-			ipa_ctx->sys[sys_idx].event.options = SPS_O_EOT;
-			ipa_ctx->sys[sys_idx].event.mode = SPS_TRIGGER_CALLBACK;
-			ipa_ctx->sys[sys_idx].event.xfer_done = NULL;
-			ipa_ctx->sys[sys_idx].event.callback = ipa_tx_notify;
-			ipa_ctx->sys[sys_idx].event.user =
-				&ipa_ctx->sys[sys_idx];
-			result =
-			   sps_register_event(ipa_ctx->ep[ipa_ep_idx].ep_hdl,
-					      &ipa_ctx->sys[sys_idx].event);
-			if (result < 0) {
-				IPAERR("tx register event error %d\n", result);
-				goto fail_register_event;
-			}
+
+		ipa_ctx->sys[sys_idx].event.options = SPS_O_EOT;
+		ipa_ctx->sys[sys_idx].event.mode = SPS_TRIGGER_CALLBACK;
+		ipa_ctx->sys[sys_idx].event.xfer_done = NULL;
+		ipa_ctx->sys[sys_idx].event.user =
+			&ipa_ctx->sys[sys_idx];
+		ipa_ctx->sys[sys_idx].event.callback =
+				IPA_CLIENT_IS_CONS(sys_in->client) ?
+					ipa_sps_irq_rx_notify :
+					ipa_sps_irq_tx_notify;
+		result = sps_register_event(ipa_ctx->ep[ipa_ep_idx].ep_hdl,
+					  &ipa_ctx->sys[sys_idx].event);
+		if (result < 0) {
+			IPAERR("register event error %d\n", result);
+			goto fail_register_event;
 		}
 	}
 
@@ -801,21 +835,25 @@
 EXPORT_SYMBOL(ipa_teardown_sys_pipe);
 
 /**
- * ipa_tx_comp() - Callback function which will call the user supplied callback
- * function to release the skb, or release it on its own if no callback function
- * was supplied.
+ * ipa_tx_comp_usr_notify_release() - Callback function which will call the
+ * user supplied callback function to release the skb, or release it on
+ * its own if no callback function was supplied.
  * @user1
  * @user2
+ *
+ * This notified callback (client_notify) is for
+ * the destination client.
+ * This function is supplied in ipa_connect.
  */
-static void ipa_tx_comp(void *user1, void *user2)
+static void ipa_tx_comp_usr_notify_release(void *user1, void *user2)
 {
 	struct sk_buff *skb = (struct sk_buff *)user1;
 	u32 ep_idx = (u32)user2;
 
 	IPADBG("skb=%p ep=%d\n", skb, ep_idx);
 
-	if (ipa_ctx->ep[ep_idx].notify)
-		ipa_ctx->ep[ep_idx].notify(ipa_ctx->ep[ep_idx].priv,
+	if (ipa_ctx->ep[ep_idx].client_notify)
+		ipa_ctx->ep[ep_idx].client_notify(ipa_ctx->ep[ep_idx].priv,
 				IPA_WRITE_DONE, (unsigned long)skb);
 	else
 		dev_kfree_skb_any(skb);
@@ -832,10 +870,20 @@
  * dst is a "valid" CONS type, then SW data-path is used. If dst is the
  * WLAN_AMPDU PROD type, then HW data-path for WLAN AMPDU is used. Anything else
  * is an error. For errors, client needs to free the skb as needed. For success,
- * IPA driver will later invoke client calback if one was supplied. That
+ * IPA driver will later invoke client callback if one was supplied. That
  * callback should free the skb. If no callback supplied, IPA driver will free
  * the skb internally
  *
+ * The function will use two descriptors for this send command
+ * (for A5_WLAN_AMPDU_PROD only one desciprtor will be sent),
+ * the first descriptor will be used to inform the IPA hardware that
+ * apps need to push data into the IPA (IP_PACKET_INIT immediate command).
+ * Once this send was done from SPS point-of-view the IPA driver will
+ * get notified by the supplied callback - ipa_sps_irq_tx_comp()
+ *
+ * ipa_sps_irq_tx_comp will call to the user supplied
+ * callback (supplied in ipa_connect())
+ *
  * Returns:	0 on success, negative on failure
  */
 int ipa_tx_dp(enum ipa_client_type dst, struct sk_buff *skb,
@@ -864,6 +912,7 @@
 			IPAERR("failed to alloc immediate command object\n");
 			goto fail_mem_alloc;
 		}
+		memset(cmd, 0x00, sizeof(*cmd));
 
 		cmd->destination_pipe_index = ipa_ep_idx;
 		if (meta && meta->mbim_stream_id_valid)
@@ -875,7 +924,7 @@
 		desc[1].pyld = skb->data;
 		desc[1].len = skb->len;
 		desc[1].type = IPA_DATA_DESC_SKB;
-		desc[1].callback = ipa_tx_comp;
+		desc[1].callback = ipa_tx_comp_usr_notify_release;
 		desc[1].user1 = skb;
 		desc[1].user2 = (void *)ipa_ep_idx;
 
@@ -887,7 +936,7 @@
 		desc[0].pyld = skb->data;
 		desc[0].len = skb->len;
 		desc[0].type = IPA_DATA_DESC_SKB;
-		desc[0].callback = ipa_tx_comp;
+		desc[0].callback = ipa_tx_comp_usr_notify_release;
 		desc[0].user1 = skb;
 		desc[0].user2 = (void *)ipa_ep_idx;
 
@@ -919,7 +968,7 @@
  * ipa_handle_rx_core() is run in polling mode. After all packets has been
  * received, the driver switches back to interrupt mode.
  */
-void ipa_handle_rx(struct work_struct *work)
+void ipa_wq_handle_rx(struct work_struct *work)
 {
 	ipa_handle_rx_core();
 	ipa_rx_switch_to_intr_mode();
@@ -962,7 +1011,7 @@
 		}
 
 		INIT_LIST_HEAD(&rx_pkt->link);
-		INIT_WORK(&rx_pkt->work, ipa_handle_rx);
+		INIT_WORK(&rx_pkt->work, ipa_wq_handle_rx);
 
 		rx_pkt->skb = __dev_alloc_skb(IPA_RX_SKB_SIZE, GFP_KERNEL);
 		if (rx_pkt->skb == NULL) {
diff --git a/drivers/platform/msm/ipa/ipa_i.h b/drivers/platform/msm/ipa/ipa_i.h
index 63ef5fb..3be2369 100644
--- a/drivers/platform/msm/ipa/ipa_i.h
+++ b/drivers/platform/msm/ipa/ipa_i.h
@@ -296,8 +296,9 @@
  * @dst_pipe_index: destination pipe index
  * @rt_tbl_idx: routing table index
  * @connect: SPS connect
- * @priv: user provided information
- * @notify: user provided CB for EP events notification
+ * @priv: user provided information which will forwarded once the user is
+ *        notified for new data avail
+ * @client_notify: user provided CB for EP events notification
  * @desc_fifo_in_pipe_mem: flag indicating if descriptors FIFO uses pipe memory
  * @data_fifo_in_pipe_mem: flag indicating if data FIFO uses pipe memory
  * @desc_fifo_pipe_mem_ofst: descriptors FIFO pipe memory offset
@@ -314,7 +315,7 @@
 	u32 rt_tbl_idx;
 	struct sps_connect connect;
 	void *priv;
-	void (*notify)(void *priv, enum ipa_dp_evt_type evt,
+	void (*client_notify)(void *priv, enum ipa_dp_evt_type evt,
 		       unsigned long data);
 	bool desc_fifo_in_pipe_mem;
 	bool data_fifo_in_pipe_mem;
@@ -357,7 +358,8 @@
 
 /**
  * struct ipa_tx_pkt_wrapper - IPA Tx packet wrapper
- * @type: info for the skb or immediate command param
+ * @type: specify if this packet is a data packet (skb) or
+ * an immediate command
  * @mem: memory buffer used by this Tx packet
  * @work: work struct for current Tx packet
  * @link: linked to the wrappers on that pipe
@@ -371,6 +373,8 @@
  * >1 and <0xFFFF for first of a "multiple" tranfer,
  * 0xFFFF for last desc, 0 for rest of "multiple' transfer
  * @bounce: va of bounce buffer
+ *
+ * This struct can wrap both data packet and immediate command packet.
  */
 struct ipa_tx_pkt_wrapper {
 	enum ipa_desc_type type;
@@ -693,8 +697,8 @@
 void ipa_replenish_rx_cache(void);
 void ipa_cleanup_rx(void);
 int ipa_cfg_filter(u32 disable);
-void ipa_write_done(struct work_struct *work);
-void ipa_handle_rx(struct work_struct *work);
+void ipa_wq_write_done(struct work_struct *work);
+void ipa_wq_handle_rx(struct work_struct *work);
 void ipa_handle_rx_core(void);
 int ipa_pipe_mem_init(u32 start_ofst, u32 size);
 int ipa_pipe_mem_alloc(u32 *ofst, u32 size);
diff --git a/drivers/platform/msm/sps/sps_bam.c b/drivers/platform/msm/sps/sps_bam.c
index a3bbb73..af421ac 100644
--- a/drivers/platform/msm/sps/sps_bam.c
+++ b/drivers/platform/msm/sps/sps_bam.c
@@ -1167,6 +1167,7 @@
 	struct sps_iovec *desc;
 	struct sps_iovec iovec;
 	u32 next_write;
+	static int show_recom;
 
 	/* Is this a BAM-to-BAM or satellite connection? */
 	if ((pipe->state & (BAM_STATE_BAM2BAM | BAM_STATE_REMOTE))) {
@@ -1199,12 +1200,24 @@
 		if (!pipe->sys.ack_xfers && pipe->polled) {
 			pipe_handler_eot(dev, pipe);
 			if (next_write == pipe->sys.acked_offset) {
+				if (!show_recom) {
+					show_recom = true;
+					SPS_ERR("sps:Client of BAM 0x%x pipe %d is recommended to have flow control",
+						BAM_ID(dev), pipe_index);
+				}
+
 				SPS_DBG2("sps:Descriptor FIFO is full for BAM "
 					"0x%x pipe %d after pipe_handler_eot",
 					BAM_ID(dev), pipe_index);
 				return SPS_ERROR;
 			}
 		} else {
+			if (!show_recom) {
+				show_recom = true;
+				SPS_ERR("sps:Client of BAM 0x%x pipe %d is recommended to have flow control.",
+					BAM_ID(dev), pipe_index);
+			}
+
 			SPS_DBG2("sps:Descriptor FIFO is full for "
 				"BAM 0x%x pipe %d", BAM_ID(dev), pipe_index);
 			return SPS_ERROR;
diff --git a/drivers/platform/msm/usb_bam.c b/drivers/platform/msm/usb_bam.c
index 8f97531..6b067e7 100644
--- a/drivers/platform/msm/usb_bam.c
+++ b/drivers/platform/msm/usb_bam.c
@@ -398,8 +398,8 @@
 			pr_err("%s: src pipe connection failure\n", __func__);
 			return ret;
 		}
+		connection->src_enabled = 1;
 	}
-	connection->src_enabled = 1;
 
 	if (dst_pipe_idx) {
 		/* open Peripheral -> USB pipe */
@@ -409,8 +409,8 @@
 			pr_err("%s: dst pipe connection failure\n", __func__);
 			return ret;
 		}
+		connection->dst_enabled = 1;
 	}
-	connection->dst_enabled = 1;
 
 	return 0;
 }
diff --git a/drivers/power/pm8921-bms.c b/drivers/power/pm8921-bms.c
index 05b47cc..a8d52b5 100644
--- a/drivers/power/pm8921-bms.c
+++ b/drivers/power/pm8921-bms.c
@@ -1800,7 +1800,7 @@
 
 	pr_debug("RUC = %duAh\n", remaining_usable_charge_uah);
 	if (fcc_uah - unusable_charge_uah <= 0) {
-		pr_warn("FCC = %duAh, UUC = %duAh forcing soc = 0\n",
+		pr_debug("FCC = %duAh, UUC = %duAh forcing soc = 0\n",
 						fcc_uah, unusable_charge_uah);
 		soc = 0;
 	} else {
@@ -1843,13 +1843,13 @@
 		soc = 100;
 
 	if (soc < 0) {
-		pr_err("bad rem_usb_chg = %d rem_chg %d,"
+		pr_debug("bad rem_usb_chg = %d rem_chg %d,"
 				"cc_uah %d, unusb_chg %d\n",
 				remaining_usable_charge_uah,
 				remaining_charge_uah,
 				cc_uah, unusable_charge_uah);
 
-		pr_err("for bad rem_usb_chg last_ocv_uv = %d"
+		pr_debug("for bad rem_usb_chg last_ocv_uv = %d"
 				"chargecycles = %d, batt_temp = %d"
 				"fcc = %d soc =%d\n",
 				chip->last_ocv_uv, chargecycles, batt_temp,
diff --git a/drivers/power/pm8921-charger.c b/drivers/power/pm8921-charger.c
index c6dd57b..bd62cf1 100644
--- a/drivers/power/pm8921-charger.c
+++ b/drivers/power/pm8921-charger.c
@@ -21,6 +21,7 @@
 #include <linux/mfd/pm8xxx/pm8xxx-adc.h>
 #include <linux/mfd/pm8xxx/ccadc.h>
 #include <linux/mfd/pm8xxx/core.h>
+#include <linux/regulator/consumer.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/bitops.h>
@@ -119,13 +120,6 @@
 	int			batt_state;
 };
 
-static int pm8921_battery_gauge_alarm_notify(struct notifier_block *nb,
-					  unsigned long status, void *unused);
-
-static struct notifier_block alarm_notifier = {
-	.notifier_call = pm8921_battery_gauge_alarm_notify,
-};
-
 static struct fsm_state_to_batt_status map[] = {
 	{FSM_STATE_OFF_0, POWER_SUPPLY_STATUS_UNKNOWN},
 	{FSM_STATE_BATFETDET_START_12, POWER_SUPPLY_STATUS_UNKNOWN},
@@ -261,10 +255,12 @@
 	struct dentry			*dent;
 	struct bms_notify		bms_notify;
 	int				*usb_trim_table;
+	struct regulator		*vreg_xoadc;
 	bool				ext_charging;
 	bool				ext_charge_done;
 	bool				iusb_fine_res;
-	bool				disable_hw_clock_switching;
+	bool				final_kickstart;
+	bool				lockup_lpm_wrkarnd;
 	DECLARE_BITMAP(enabled_irqs, PM_CHG_MAX_INTS);
 	struct work_struct		battery_id_valid_work;
 	int64_t				batt_id_min;
@@ -280,6 +276,7 @@
 	struct delayed_work		eoc_work;
 	struct delayed_work		unplug_check_work;
 	struct delayed_work		vin_collapse_check_work;
+	struct delayed_work		btc_override_work;
 	struct wake_lock		eoc_wake_lock;
 	enum pm8921_chg_cold_thr	cold_thr;
 	enum pm8921_chg_hot_thr		hot_thr;
@@ -291,6 +288,11 @@
 	int				recent_reported_soc;
 	int				battery_less_hardware;
 	int				ibatmax_max_adj_ma;
+	int				btc_override;
+	int				btc_override_cold_decidegc;
+	int				btc_override_hot_decidegc;
+	int				btc_delay_ms;
+	bool				btc_panic_if_cant_stop_chg;
 };
 
 /* user space parameter to limit usb current */
@@ -306,6 +308,71 @@
 
 static struct pm8921_chg_chip *the_chip;
 
+static DEFINE_SPINLOCK(lpm_lock);
+#define LPM_ENABLE_BIT	BIT(2)
+static int pm8921_chg_set_lpm(struct pm8921_chg_chip *chip, int enable)
+{
+	int rc;
+	u8 reg;
+
+	rc = pm8xxx_readb(chip->dev->parent, CHG_CNTRL, &reg);
+	if (rc) {
+		pr_err("pm8xxx_readb failed: addr=%03X, rc=%d\n",
+				CHG_CNTRL, rc);
+		return rc;
+	}
+	reg &= ~LPM_ENABLE_BIT;
+	reg |= (enable ? LPM_ENABLE_BIT : 0);
+
+	rc = pm8xxx_writeb(chip->dev->parent, CHG_CNTRL, reg);
+	if (rc) {
+		pr_err("pm_chg_write failed: addr=%03X, rc=%d\n",
+				CHG_CNTRL, rc);
+		return rc;
+	}
+
+	return rc;
+}
+
+static int pm_chg_write(struct pm8921_chg_chip *chip, u16 addr, u8 reg)
+{
+	int rc;
+	unsigned long flags = 0;
+
+	/* Disable LPM */
+	if (chip->lockup_lpm_wrkarnd) {
+		spin_lock_irqsave(&lpm_lock, flags);
+
+		/*
+		 * This write could have initiated right after a previous write.
+		 * Allow time to settle to go in to lpm from the previous write
+		 */
+		udelay(200);
+		rc = pm8921_chg_set_lpm(chip, 0);
+		if (rc)
+			goto lpm_err;
+
+		/* Wait to come out of LPM */
+		udelay(200);
+	}
+
+	rc = pm8xxx_writeb(chip->dev->parent, addr, reg);
+	if (rc) {
+		pr_err("pm_chg_write failed: addr=%03X, rc=%d\n", addr, rc);
+		goto lpm_err;
+	}
+
+	/* Enable LPM */
+	if (chip->lockup_lpm_wrkarnd)
+		rc = pm8921_chg_set_lpm(chip, 1);
+
+lpm_err:
+	if (chip->lockup_lpm_wrkarnd)
+		spin_unlock_irqrestore(&lpm_lock, flags);
+
+	return rc;
+}
+
 static int pm_chg_masked_write(struct pm8921_chg_chip *chip, u16 addr,
 							u8 mask, u8 val)
 {
@@ -319,9 +386,9 @@
 	}
 	reg &= ~mask;
 	reg |= val & mask;
-	rc = pm8xxx_writeb(chip->dev->parent, addr, reg);
+	rc = pm_chg_write(chip, addr, reg);
 	if (rc) {
-		pr_err("pm8xxx_writeb failed: addr=%03X, rc=%d\n", addr, rc);
+		pr_err("pm_chg_write failed: addr=%03X, rc=%d\n", addr, rc);
 		return rc;
 	}
 	return 0;
@@ -333,6 +400,23 @@
 					chip->pmic_chg_irq[irq_id]);
 }
 
+static int is_chg_on_bat(struct pm8921_chg_chip *chip)
+{
+	return !(pm_chg_get_rt_status(chip, DCIN_VALID_IRQ)
+			|| pm_chg_get_rt_status(chip, USBIN_VALID_IRQ));
+}
+
+static void pm8921_chg_bypass_bat_gone_debounce(struct pm8921_chg_chip *chip,
+		int bypass)
+{
+	int rc;
+
+	rc = pm_chg_write(chip, COMPARATOR_OVERRIDE, bypass ? 0x89 : 0x88);
+	if (rc) {
+		pr_err("Failed to set bypass bit to %d rc=%d\n", bypass, rc);
+	}
+}
+
 /* Treat OverVoltage/UnderVoltage as source missing */
 static int is_usb_chg_plugged_in(struct pm8921_chg_chip *chip)
 {
@@ -358,14 +442,14 @@
 	int err, ret = 0;
 
 	temp = CAPTURE_FSM_STATE_CMD;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	err = pm_chg_write(chip, CHG_TEST, temp);
 	if (err) {
 		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
 		return err;
 	}
 
 	temp = READ_BANK_7;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	err = pm_chg_write(chip, CHG_TEST, temp);
 	if (err) {
 		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
 		return err;
@@ -380,7 +464,7 @@
 	ret = temp & 0xF;
 
 	temp = READ_BANK_4;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	err = pm_chg_write(chip, CHG_TEST, temp);
 	if (err) {
 		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
 		return err;
@@ -403,7 +487,7 @@
 	int err;
 
 	temp = READ_BANK_6;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	err = pm_chg_write(chip, CHG_TEST, temp);
 	if (err) {
 		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
 		return err;
@@ -485,7 +569,7 @@
 	}
 
 	pr_debug("voltage=%d setting %02x\n", voltage, temp);
-	return pm8xxx_writeb(chip->dev->parent, CHG_VDD_MAX, temp);
+	return pm_chg_write(chip, CHG_VDD_MAX, temp);
 }
 
 static int pm_chg_vddmax_get(struct pm8921_chg_chip *chip, int *voltage)
@@ -754,7 +838,7 @@
 	0x13,
 	0x14,
 	0x13,
-	0x16,
+	0x3,
 	0x1A,
 	0x1D,
 	0x1D,
@@ -783,15 +867,18 @@
 	{1600, 0xF},
 };
 
-#define REG_SBI_CONFIG		0x04F
-#define PAGE3_ENABLE_MASK	0x6
-#define USB_OVP_TRIM_MASK	0x3F
-#define USB_OVP_TRIM_MIN	0x00
+#define REG_SBI_CONFIG			0x04F
+#define PAGE3_ENABLE_MASK		0x6
+#define USB_OVP_TRIM_MASK		0x3F
+#define USB_OVP_TRIM_PM8917_MASK	0x7F
+#define USB_OVP_TRIM_MIN		0x00
 #define REG_USB_OVP_TRIM_ORIG_LSB	0x10A
 #define REG_USB_OVP_TRIM_ORIG_MSB	0x09C
+#define REG_USB_OVP_TRIM_PM8917		0x2B5
+#define REG_USB_OVP_TRIM_PM8917_BIT	BIT(0)
 static int pm_chg_usb_trim(struct pm8921_chg_chip *chip, int index)
 {
-	u8 temp, sbi_config, msb, lsb;
+	u8 temp, sbi_config, msb, lsb, mask;
 	s8 trim;
 	int rc = 0;
 	static u8 usb_trim_reg_orig = 0xFF;
@@ -818,6 +905,19 @@
 		msb = msb >> 5;
 		lsb = lsb >> 5;
 		usb_trim_reg_orig = msb << 3 | lsb;
+
+		if (pm8xxx_get_version(chip->dev->parent)
+				== PM8XXX_VERSION_8917) {
+			rc = pm8xxx_readb(chip->dev->parent,
+					REG_USB_OVP_TRIM_PM8917, &msb);
+			if (rc) {
+				pr_err("error = %d reading config reg\n", rc);
+				return rc;
+			}
+
+			msb = msb & REG_USB_OVP_TRIM_PM8917_BIT;
+			usb_trim_reg_orig |= msb << 6;
+		}
 	}
 
 	/* use the original trim value */
@@ -837,19 +937,24 @@
 	}
 
 	temp = sbi_config | PAGE3_ENABLE_MASK;
-	rc = pm8xxx_writeb(chip->dev->parent, REG_SBI_CONFIG, temp);
+	rc = pm_chg_write(chip, REG_SBI_CONFIG, temp);
 	if (rc) {
 		pr_err("error = %d writing sbi config reg\n", rc);
 		return rc;
 	}
 
-	rc = pm_chg_masked_write(chip, USB_OVP_TRIM, USB_OVP_TRIM_MASK, trim);
+	mask = USB_OVP_TRIM_MASK;
+
+	if (pm8xxx_get_version(chip->dev->parent) == PM8XXX_VERSION_8917)
+		mask = USB_OVP_TRIM_PM8917_MASK;
+
+	rc = pm_chg_masked_write(chip, USB_OVP_TRIM, mask, trim);
 	if (rc) {
 		pr_err("error = %d writing USB_OVP_TRIM\n", rc);
 		return rc;
 	}
 
-	rc = pm8xxx_writeb(chip->dev->parent, REG_SBI_CONFIG, sbi_config);
+	rc = pm_chg_write(chip, REG_SBI_CONFIG, sbi_config);
 	if (rc) {
 		pr_err("error = %d writing sbi config reg\n", rc);
 		return rc;
@@ -1337,7 +1442,7 @@
 		return 0;
 
 	/* enable usbin valid comparator and remove force usb ovp fet off */
-	rc = pm8xxx_writeb(chip->dev->parent, USB_OVP_TEST, 0xB2);
+	rc = pm_chg_write(chip, USB_OVP_TEST, 0xB2);
 	if (rc < 0) {
 		pr_err("Failed to write 0xB2 to USB_OVP_TEST rc = %d\n", rc);
 		return rc;
@@ -1356,7 +1461,7 @@
 		return 0;
 
 	/* disable usbin valid comparator and force usb ovp fet off */
-	rc = pm8xxx_writeb(chip->dev->parent, USB_OVP_TEST, 0xB3);
+	rc = pm_chg_write(chip, USB_OVP_TEST, 0xB3);
 	if (rc < 0) {
 		pr_err("Failed to write 0xB3 to USB_OVP_TEST rc = %d\n", rc);
 		return rc;
@@ -1939,8 +2044,7 @@
 	if (disable) {
 		pr_warn("Disabling input current limit!\n");
 
-		return pm8xxx_writeb(the_chip->dev->parent,
-			 CHG_BUCK_CTRL_TEST3, 0xF2);
+		return pm_chg_write(the_chip, CHG_BUCK_CTRL_TEST3, 0xF2);
 	}
 	return 0;
 }
@@ -2125,76 +2229,86 @@
 	return get_prop_batt_temp(the_chip);
 }
 
-static int pm8921_charger_enable_batt_alarm(struct pm8921_chg_chip *chip)
+static int pm8921_apply_19p2mhz_kickstart(struct pm8921_chg_chip *chip)
 {
-	int rc = 0;
+	int err;
+	u8 temp;
+	unsigned long flags = 0;
 
-	rc = pm8xxx_batt_alarm_disable(PM8XXX_BATT_ALARM_UPPER_COMPARATOR);
-	if (!rc)
-		rc = pm8xxx_batt_alarm_enable(
-			PM8XXX_BATT_ALARM_LOWER_COMPARATOR);
-	if (rc) {
-		pr_err("unable to set batt alarm state rc=%d\n", rc);
-		return rc;
+	spin_lock_irqsave(&lpm_lock, flags);
+	err = pm8921_chg_set_lpm(chip, 0);
+	if (err) {
+		pr_err("Error settig LPM rc=%d\n", err);
+		goto kick_err;
 	}
 
-	return rc;
-}
-static int pm8921_charger_configure_batt_alarm(struct pm8921_chg_chip *chip)
-{
-	int rc = 0;
-
-	rc = pm8xxx_batt_alarm_disable(PM8XXX_BATT_ALARM_UPPER_COMPARATOR);
-	if (!rc)
-		rc = pm8xxx_batt_alarm_disable(
-			PM8XXX_BATT_ALARM_LOWER_COMPARATOR);
-	if (rc) {
-		pr_err("unable to set batt alarm state rc=%d\n", rc);
-		return rc;
+	temp  = 0xD1;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
 	}
 
-	/*
-	 * The batt-alarm driver requires sane values for both min / max,
-	 * regardless of whether they're both activated.
-	 */
-	rc = pm8xxx_batt_alarm_threshold_set(
-			PM8XXX_BATT_ALARM_LOWER_COMPARATOR,
-					chip->alarm_low_mv);
-	if (!rc)
-		rc = pm8xxx_batt_alarm_threshold_set(
-			PM8XXX_BATT_ALARM_UPPER_COMPARATOR,
-					chip->alarm_high_mv);
-	if (rc) {
-		pr_err("unable to set batt alarm threshold rc=%d\n", rc);
-		return rc;
+	temp  = 0xD3;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
 	}
 
-	rc = pm8xxx_batt_alarm_hold_time_set(
-				PM8XXX_BATT_ALARM_HOLD_TIME_16_MS);
-	if (rc) {
-		pr_err("unable to set batt alarm hold time rc=%d\n", rc);
-		return rc;
+	temp  = 0xD1;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
 	}
 
-	/* PWM enabled at 2Hz */
-	rc = pm8xxx_batt_alarm_pwm_rate_set(1, 7, 4);
-	if (rc) {
-		pr_err("unable to set batt alarm pwm rate rc=%d\n", rc);
-		return rc;
+	temp  = 0xD5;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
 	}
 
-	rc = pm8xxx_batt_alarm_register_notifier(&alarm_notifier);
-	if (rc) {
-		pr_err("unable to register alarm notifier rc=%d\n", rc);
-		return rc;
+	/* Wait a few clock cycles before re-enabling hw clock switching */
+	udelay(183);
+
+	temp  = 0xD1;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
 	}
 
-	return rc;
+	temp  = 0xD0;
+	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
+	if (err) {
+		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
+		goto kick_err;
+	}
+
+	/* Wait for few clock cycles before re-enabling LPM */
+	udelay(32);
+
+kick_err:
+	err = pm8921_chg_set_lpm(chip, 1);
+	if (err)
+		pr_err("Error settig LPM rc=%d\n", err);
+
+	spin_unlock_irqrestore(&lpm_lock, flags);
+
+	return err;
 }
 
 static void handle_usb_insertion_removal(struct pm8921_chg_chip *chip)
 {
-	int usb_present;
+	int usb_present, rc = 0;
+
+	if (chip->lockup_lpm_wrkarnd) {
+		rc = pm8921_apply_19p2mhz_kickstart(chip);
+		if (rc)
+			pr_err("Failed to apply kickstart rc=%d\n", rc);
+	}
 
 	pm_chg_failed_clear(chip, 1);
 	usb_present = is_usb_chg_plugged_in(chip);
@@ -2204,6 +2318,11 @@
 		power_supply_changed(&chip->usb_psy);
 		power_supply_changed(&chip->batt_psy);
 		pm8921_bms_calibrate_hkadc();
+
+		/* Enable/disable bypass if charger is on battery */
+		if (chip->lockup_lpm_wrkarnd)
+			pm8921_chg_bypass_bat_gone_debounce(chip,
+				is_chg_on_bat(chip));
 	}
 	if (usb_present) {
 		schedule_delayed_work(&chip->unplug_check_work,
@@ -2219,6 +2338,10 @@
 
 static void handle_stop_ext_chg(struct pm8921_chg_chip *chip)
 {
+	if (chip->lockup_lpm_wrkarnd)
+		/* Enable bypass if charger is on battery */
+		pm8921_chg_bypass_bat_gone_debounce(chip, is_chg_on_bat(chip));
+
 	if (!chip->ext_psy) {
 		pr_debug("external charger not registered.\n");
 		return;
@@ -2245,10 +2368,13 @@
 	int dc_present;
 	int batt_present;
 	int batt_temp_ok;
-	int vbat_ov;
 	unsigned long delay =
 		round_jiffies_relative(msecs_to_jiffies(EOC_CHECK_PERIOD_MS));
 
+	/* Disable bypass if charger connected and not running on bat */
+	if (chip->lockup_lpm_wrkarnd)
+		pm8921_chg_bypass_bat_gone_debounce(chip, is_chg_on_bat(chip));
+
 	if (!chip->ext_psy) {
 		pr_debug("external charger not registered.\n");
 		return;
@@ -2259,7 +2385,7 @@
 		return;
 	}
 
-	dc_present = is_dc_chg_plugged_in(the_chip);
+	dc_present = is_dc_chg_plugged_in(chip);
 	batt_present = pm_chg_get_rt_status(chip, BATT_INSERTED_IRQ);
 	batt_temp_ok = pm_chg_get_rt_status(chip, BAT_TEMP_OK_IRQ);
 
@@ -2279,12 +2405,6 @@
 	/* Force BATFET=ON */
 	pm8921_disable_source_current(true);
 
-	vbat_ov = pm_chg_get_rt_status(chip, VBAT_OV_IRQ);
-	if (vbat_ov) {
-		pr_warn("%s. battery over voltage.\n", __func__);
-		return;
-	}
-
 	schedule_delayed_work(&chip->unplug_check_work,
 			msecs_to_jiffies(UNPLUG_CHECK_RAMP_MS));
 
@@ -2300,6 +2420,10 @@
 	 */
 	schedule_delayed_work(&chip->eoc_work, delay);
 	wake_lock(&chip->eoc_wake_lock);
+	if (chip->btc_override)
+		schedule_delayed_work(&chip->btc_override_work,
+				round_jiffies_relative(msecs_to_jiffies
+					(chip->btc_delay_ms)));
 	/* Update battery charging LEDs and user space battery info */
 	power_supply_changed(&chip->batt_psy);
 }
@@ -2309,86 +2433,31 @@
 	u8 temp;
 	int rc;
 
-	rc = pm8xxx_writeb(chip->dev->parent, ovptestreg, 0x30);
+	rc = pm_chg_write(chip, ovptestreg, 0x30);
 	if (rc) {
-		pr_err("Failed to write 0x30 to OVP_TEST rc = %d\n", rc);
+		pr_err("Failed to write 0x30 to ovptestreg rc = %d\n", rc);
 		return;
 	}
 	rc = pm8xxx_readb(chip->dev->parent, ovptestreg, &temp);
 	if (rc) {
-		pr_err("Failed to read from OVP_TEST rc = %d\n", rc);
+		pr_err("Failed to read from ovptestreg rc = %d\n", rc);
 		return;
 	}
 	/* set ovp fet disable bit and the write bit */
 	temp |= 0x81;
-	rc = pm8xxx_writeb(chip->dev->parent, ovptestreg, temp);
+	rc = pm_chg_write(chip, ovptestreg, temp);
 	if (rc) {
-		pr_err("Failed to write 0x%x OVP_TEST rc=%d\n", temp, rc);
+		pr_err("Failed to write 0x%x ovptestreg rc=%d\n", temp, rc);
 		return;
 	}
 }
 
-static int pm8921_battery_gauge_alarm_notify(struct notifier_block *nb,
-		unsigned long status, void *unused)
-{
-	int rc;
-
-	pr_info("status: %lu\n", status);
-
-	/* Check if called before init */
-
-	switch (status) {
-	case 0:
-		pr_err("spurious interrupt\n");
-		break;
-	/* expected case - trip of low threshold */
-	case 1:
-		if (!the_chip) {
-			pr_err("not initialized\n");
-			return -EINVAL;
-		}
-
-		the_chip->disable_hw_clock_switching = 1;
-
-		rc = pm8xxx_batt_alarm_disable(
-				PM8XXX_BATT_ALARM_LOWER_COMPARATOR);
-		if (!rc)
-			rc = pm8xxx_batt_alarm_enable(
-				PM8XXX_BATT_ALARM_UPPER_COMPARATOR);
-		if (rc)
-			pr_err("unable to set alarm state rc=%d\n", rc);
-		break;
-	case 2:
-		if (!the_chip) {
-			pr_err("not initialized\n");
-			return -EINVAL;
-		}
-
-		the_chip->disable_hw_clock_switching = 0;
-
-		rc = pm8xxx_batt_alarm_disable(
-				PM8XXX_BATT_ALARM_UPPER_COMPARATOR);
-		if (!rc)
-			rc = pm8xxx_batt_alarm_enable(
-				PM8XXX_BATT_ALARM_LOWER_COMPARATOR);
-		if (rc)
-			pr_err("unable to set alarm state rc=%d\n", rc);
-
-		pr_err("trip of high threshold\n");
-		break;
-	default:
-		pr_err("error received\n");
-	};
-
-	return 0;
-}
-
 static void turn_on_ovp_fet(struct pm8921_chg_chip *chip, u16 ovptestreg)
 {
 	u8 temp;
 	int rc;
 
-	rc = pm8xxx_writeb(chip->dev->parent, ovptestreg, 0x30);
+	rc = pm_chg_write(chip, ovptestreg, 0x30);
 	if (rc) {
 		pr_err("Failed to write 0x30 to OVP_TEST rc = %d\n", rc);
 		return;
@@ -2401,7 +2470,7 @@
 	/* unset ovp fet disable bit and set the write bit */
 	temp &= 0xFE;
 	temp |= 0x80;
-	rc = pm8xxx_writeb(chip->dev->parent, ovptestreg, temp);
+	rc = pm_chg_write(chip, ovptestreg, temp);
 	if (rc) {
 		pr_err("Failed to write 0x%x to OVP_TEST rc = %d\n",
 								temp, rc);
@@ -2614,12 +2683,6 @@
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t vbat_ov_irq_handler(int irq, void *data)
-{
-	pr_debug("fsm_state=%d\n", pm_chg_get_fsm_state(data));
-	return IRQ_HANDLED;
-}
-
 static irqreturn_t chgwdog_irq_handler(int irq, void *data)
 {
 	pr_debug("fsm_state=%d\n", pm_chg_get_fsm_state(data));
@@ -2706,7 +2769,7 @@
 	u8 temp;
 	int rc;
 
-	rc = pm8xxx_writeb(chip->dev->parent, CHG_BUCK_CTRL_TEST3, 0x40);
+	rc = pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, 0x40);
 	if (rc) {
 		pr_err("Failed to write 0x70 to CTRL_TEST3 rc = %d\n", rc);
 		return;
@@ -2722,7 +2785,7 @@
 	temp |= (u8)pon_time_ns;
 	/* write enable bank 4 */
 	temp |= 0x80;
-	rc = pm8xxx_writeb(chip->dev->parent, CHG_BUCK_CTRL_TEST3, temp);
+	rc = pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, temp);
 	if (rc) {
 		pr_err("Failed to write 0x%x to CTRL_TEST3 rc=%d\n", temp, rc);
 		return;
@@ -2748,20 +2811,18 @@
 	struct delayed_work *dwork = to_delayed_work(work);
 	struct pm8921_chg_chip *chip = container_of(dwork,
 				struct pm8921_chg_chip, unplug_check_work);
-	u8 reg_loop, active_path;
+	u8 reg_loop = 0, active_path;
 	int rc, ibat, active_chg_plugged_in, usb_ma;
 	int chg_gone = 0;
 	bool ramp = false;
 
-	reg_loop = 0;
-
 	rc = pm8xxx_readb(chip->dev->parent, PBL_ACCESS1, &active_path);
 	if (rc) {
 		pr_err("Failed to read PBL_ACCESS1 rc=%d\n", rc);
 		return;
 	}
-	chip->active_path = active_path;
 
+	chip->active_path = active_path;
 	active_chg_plugged_in = is_active_chg_plugged_in(chip, active_path);
 	pr_debug("active_path = 0x%x, active_chg_plugged_in = %d\n",
 			active_path, active_chg_plugged_in);
@@ -2788,11 +2849,28 @@
 				pm_chg_get_fsm_state(chip),
 				get_prop_batt_current(chip)
 				);
+			if (chip->lockup_lpm_wrkarnd) {
+				rc = pm8921_apply_19p2mhz_kickstart(chip);
+				if (rc)
+					pr_err("Failed kickstart rc=%d\n", rc);
+
+				/*
+				 * Make sure kickstart happens at least 200 ms
+				 * after charger has been removed.
+				 */
+				if (chip->final_kickstart) {
+					chip->final_kickstart = false;
+					goto check_again_later;
+				}
+			}
 			return;
 		} else {
 			goto check_again_later;
 		}
 	}
+
+	chip->final_kickstart = true;
+
 	/* AICL only for usb wall charger */
 	if ((active_path & USB_ACTIVE_BIT) && usb_target_ma > 0) {
 		reg_loop = pm_chg_get_regulation_loop(chip);
@@ -2912,6 +2990,13 @@
 				      round_jiffies_relative(msecs_to_jiffies
 						     (EOC_CHECK_PERIOD_MS)));
 	}
+	if (high_transition
+		&& chip->btc_override
+		&& !delayed_work_pending(&chip->btc_override_work)) {
+		schedule_delayed_work(&chip->btc_override_work,
+					round_jiffies_relative(msecs_to_jiffies
+						(chip->btc_delay_ms)));
+	}
 	power_supply_changed(&chip->batt_psy);
 	bms_notify_check(chip);
 	return IRQ_HANDLED;
@@ -3074,6 +3159,11 @@
 		else
 			handle_stop_ext_chg(chip);
 	} else {
+		if (chip->lockup_lpm_wrkarnd)
+			/* if no external supply call bypass debounce here */
+			pm8921_chg_bypass_bat_gone_debounce(chip,
+				is_chg_on_bat(chip));
+
 		if (dc_present)
 			schedule_delayed_work(&chip->unplug_check_work,
 				msecs_to_jiffies(UNPLUG_CHECK_WAIT_PERIOD_MS));
@@ -3404,6 +3494,155 @@
 	return CHG_FINISHED;
 }
 
+#define COMP_OVERRIDE_HOT_BANK	6
+#define COMP_OVERRIDE_COLD_BANK	7
+#define COMP_OVERRIDE_BIT  BIT(1)
+static int pm_chg_override_cold(struct pm8921_chg_chip *chip, int flag)
+{
+	u8 val;
+	int rc = 0;
+
+	val = 0x80 | COMP_OVERRIDE_COLD_BANK << 2 | COMP_OVERRIDE_BIT;
+
+	if (flag)
+		val |= 0x01;
+
+	rc = pm_chg_write(chip, COMPARATOR_OVERRIDE, val);
+	if (rc < 0)
+		pr_err("Could not write 0x%x to override rc = %d\n", val, rc);
+
+	pr_debug("btc cold = %d val = 0x%x\n", flag, val);
+	return rc;
+}
+
+static int pm_chg_override_hot(struct pm8921_chg_chip *chip, int flag)
+{
+	u8 val;
+	int rc = 0;
+
+	val = 0x80 | COMP_OVERRIDE_HOT_BANK << 2 | COMP_OVERRIDE_BIT;
+
+	if (flag)
+		val |= 0x01;
+
+	rc = pm_chg_write(chip, COMPARATOR_OVERRIDE, val);
+	if (rc < 0)
+		pr_err("Could not write 0x%x to override rc = %d\n", val, rc);
+
+	pr_debug("btc hot = %d val = 0x%x\n", flag, val);
+	return rc;
+}
+
+static void __devinit pm8921_chg_btc_override_init(struct pm8921_chg_chip *chip)
+{
+	int rc = 0;
+	u8 reg;
+	u8 val;
+
+	val = COMP_OVERRIDE_HOT_BANK << 2;
+	rc = pm_chg_write(chip, COMPARATOR_OVERRIDE, val);
+	if (rc < 0) {
+		pr_err("Could not write 0x%x to override rc = %d\n", val, rc);
+		goto cold_init;
+	}
+	rc = pm8xxx_readb(chip->dev->parent, COMPARATOR_OVERRIDE, &reg);
+	if (rc < 0) {
+		pr_err("Could not read bank %d of override rc = %d\n",
+				COMP_OVERRIDE_HOT_BANK, rc);
+		goto cold_init;
+	}
+	if ((reg & COMP_OVERRIDE_BIT) != COMP_OVERRIDE_BIT) {
+		/* for now override it as not hot */
+		rc = pm_chg_override_hot(chip, 0);
+		if (rc < 0)
+			pr_err("Could not override hot rc = %d\n", rc);
+	}
+
+cold_init:
+	val = COMP_OVERRIDE_COLD_BANK << 2;
+	rc = pm_chg_write(chip, COMPARATOR_OVERRIDE, val);
+	if (rc < 0) {
+		pr_err("Could not write 0x%x to override rc = %d\n", val, rc);
+		return;
+	}
+	rc = pm8xxx_readb(chip->dev->parent, COMPARATOR_OVERRIDE, &reg);
+	if (rc < 0) {
+		pr_err("Could not read bank %d of override rc = %d\n",
+				COMP_OVERRIDE_COLD_BANK, rc);
+		return;
+	}
+	if ((reg & COMP_OVERRIDE_BIT) != COMP_OVERRIDE_BIT) {
+		/* for now override it as not cold */
+		rc = pm_chg_override_cold(chip, 0);
+		if (rc < 0)
+			pr_err("Could not override cold rc = %d\n", rc);
+	}
+}
+
+static void btc_override_worker(struct work_struct *work)
+{
+	int decidegc;
+	int temp;
+	int rc = 0;
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct pm8921_chg_chip *chip = container_of(dwork,
+				struct pm8921_chg_chip, btc_override_work);
+
+	if (!chip->btc_override) {
+		pr_err("called when not enabled\n");
+		return;
+	}
+
+	decidegc = get_prop_batt_temp(chip);
+
+	pr_debug("temp=%d\n", decidegc);
+
+	temp = pm_chg_get_rt_status(chip, BATTTEMP_HOT_IRQ);
+	if (temp) {
+		if (decidegc < chip->btc_override_hot_decidegc)
+			/* stop forcing batt hot */
+			rc = pm_chg_override_hot(chip, 0);
+			if (rc)
+				pr_err("Couldnt write 0 to hot comp\n");
+	} else {
+		if (decidegc >= chip->btc_override_hot_decidegc)
+			/* start forcing batt hot */
+			rc = pm_chg_override_hot(chip, 1);
+			if (rc && chip->btc_panic_if_cant_stop_chg)
+				panic("Couldnt override comps to stop chg\n");
+	}
+
+	temp = pm_chg_get_rt_status(chip, BATTTEMP_COLD_IRQ);
+	if (temp) {
+		if (decidegc > chip->btc_override_cold_decidegc)
+			/* stop forcing batt cold */
+			rc = pm_chg_override_cold(chip, 0);
+			if (rc)
+				pr_err("Couldnt write 0 to cold comp\n");
+	} else {
+		if (decidegc <= chip->btc_override_cold_decidegc)
+			/* start forcing batt cold */
+			rc = pm_chg_override_cold(chip, 1);
+			if (rc && chip->btc_panic_if_cant_stop_chg)
+				panic("Couldnt override comps to stop chg\n");
+	}
+
+	if ((is_dc_chg_plugged_in(the_chip) || is_usb_chg_plugged_in(the_chip))
+		&& get_prop_batt_status(chip) != POWER_SUPPLY_STATUS_FULL) {
+		schedule_delayed_work(&chip->btc_override_work,
+					round_jiffies_relative(msecs_to_jiffies
+						(chip->btc_delay_ms)));
+		return;
+	}
+
+	rc = pm_chg_override_hot(chip, 0);
+	if (rc)
+		pr_err("Couldnt write 0 to hot comp\n");
+	rc = pm_chg_override_cold(chip, 0);
+	if (rc)
+		pr_err("Couldnt write 0 to cold comp\n");
+}
+
 /**
  * eoc_worker - internal function to check if battery EOC
  *		has happened
@@ -3445,18 +3684,6 @@
 		goto eoc_worker_stop;
 	}
 
-	/* If the disable hw clock switching
-	 * flag was set it can now be unset. Also, re-enable
-	 * the battery alarm to set the flag again when needed
-	 */
-	if (chip->disable_hw_clock_switching) {
-		/* Unset the hw clock switching flag */
-		chip->disable_hw_clock_switching = 0;
-
-		if (pm8921_charger_enable_batt_alarm(chip))
-			pr_err("couldn't set up batt alarm!\n");
-	}
-
 	if (end == CHG_FINISHED) {
 		count++;
 	} else {
@@ -3697,7 +3924,6 @@
 						batt_inserted_irq_handler),
 	CHG_IRQ(VBATDET_LOW_IRQ, IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
 						vbatdet_low_irq_handler),
-	CHG_IRQ(VBAT_OV_IRQ, IRQF_TRIGGER_RISING, vbat_ov_irq_handler),
 	CHG_IRQ(CHGWDOG_IRQ, IRQF_TRIGGER_RISING, chgwdog_irq_handler),
 	CHG_IRQ(VCP_IRQ, IRQF_TRIGGER_RISING, vcp_irq_handler),
 	CHG_IRQ(ATCDONE_IRQ, IRQF_TRIGGER_RISING, atcdone_irq_handler),
@@ -3769,91 +3995,6 @@
 	return -EINVAL;
 }
 
-static void pm8921_chg_force_19p2mhz_clk(struct pm8921_chg_chip *chip)
-{
-	int err;
-	u8 temp;
-
-	temp  = 0xD1;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD3;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD1;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD5;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	udelay(183);
-
-	temp  = 0xD1;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD0;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-	udelay(32);
-
-	temp  = 0xD1;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD3;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-}
-
-static void pm8921_chg_set_hw_clk_switching(struct pm8921_chg_chip *chip)
-{
-	int err;
-	u8 temp;
-
-	temp  = 0xD1;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-
-	temp  = 0xD0;
-	err = pm8xxx_writeb(chip->dev->parent, CHG_TEST, temp);
-	if (err) {
-		pr_err("Error %d writing %d to addr %d\n", err, temp, CHG_TEST);
-		return;
-	}
-}
-
 #define VREF_BATT_THERM_FORCE_ON	BIT(7)
 static void detect_battery_removal(struct pm8921_chg_chip *chip)
 {
@@ -3884,14 +4025,15 @@
 #define DEFAULT_SAFETY_MINUTES	500
 static int __devinit pm8921_chg_hw_init(struct pm8921_chg_chip *chip)
 {
-	int rc;
-	int vdd_safe;
 	u8 subrev;
-	int fcc_uah;
-	int safety_time = DEFAULT_SAFETY_MINUTES;
+	int rc, vdd_safe, fcc_uah, safety_time = DEFAULT_SAFETY_MINUTES;
 
-	/* forcing 19p2mhz before accessing any charger registers */
-	pm8921_chg_force_19p2mhz_clk(chip);
+	spin_lock_init(&lpm_lock);
+	rc = pm8921_apply_19p2mhz_kickstart(chip);
+	if (rc) {
+		pr_err("Failed to apply kickstart rc=%d\n", rc);
+		return rc;
+	}
 
 	detect_battery_removal(chip);
 
@@ -3967,6 +4109,10 @@
 						1000 * MIN_CHARGE_CURRENT_MA);
 		/* add 20 minutes of buffer time */
 		safety_time += 20;
+
+		/* make sure we do not exceed the maximum programmable time */
+		if (safety_time > PM8921_CHG_TCHG_MAX)
+			safety_time = PM8921_CHG_TCHG_MAX;
 	}
 
 	rc = pm_chg_tchg_max_set(chip, safety_time);
@@ -4010,10 +4156,10 @@
 	}
 	/* switch to a 3.2Mhz for the buck */
 	if (pm8xxx_get_revision(chip->dev->parent) >= PM8XXX_REVISION_8038_1p0)
-		rc = pm8xxx_writeb(chip->dev->parent,
+		rc = pm_chg_write(chip,
 			CHG_BUCK_CLOCK_CTRL_8038, 0x15);
 	else
-		rc = pm8xxx_writeb(chip->dev->parent,
+		rc = pm_chg_write(chip,
 			CHG_BUCK_CLOCK_CTRL, 0x15);
 
 	if (rc) {
@@ -4086,30 +4232,29 @@
 		}
 		/* Check if die 3.0.1 is present */
 		if (subrev == 0x1)
-			pm8xxx_writeb(chip->dev->parent,
-				CHG_BUCK_CTRL_TEST3, 0xA4);
+			pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, 0xA4);
 		else
-			pm8xxx_writeb(chip->dev->parent,
-				CHG_BUCK_CTRL_TEST3, 0xAC);
+			pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, 0xAC);
 	}
 
 	/* Enable isub_fine resolution AICL for PM8917 */
 	if (pm8xxx_get_version(chip->dev->parent) == PM8XXX_VERSION_8917) {
 		chip->iusb_fine_res = true;
-		if (chip->uvd_voltage_mv)
+		if (chip->uvd_voltage_mv) {
 			rc = pm_chg_uvd_threshold_set(chip,
 					chip->uvd_voltage_mv);
 			if (rc) {
 				pr_err("Failed to set UVD threshold %drc=%d\n",
 						chip->uvd_voltage_mv, rc);
-			return rc;
+				return rc;
+			}
 		}
 	}
 
-	pm8xxx_writeb(chip->dev->parent, CHG_BUCK_CTRL_TEST3, 0xD9);
+	pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, 0xD9);
 
 	/* Disable EOC FSM processing */
-	pm8xxx_writeb(chip->dev->parent, CHG_BUCK_CTRL_TEST3, 0x91);
+	pm_chg_write(chip, CHG_BUCK_CTRL_TEST3, 0x91);
 
 	rc = pm_chg_masked_write(chip, CHG_CNTRL, VREF_BATT_THERM_FORCE_ON,
 						VREF_BATT_THERM_FORCE_ON);
@@ -4128,6 +4273,45 @@
 		return rc;
 	}
 
+	if (pm8xxx_get_version(chip->dev->parent) == PM8XXX_VERSION_8921) {
+		/* Clear kickstart */
+		rc = pm8xxx_writeb(chip->dev->parent, CHG_TEST, 0xD0);
+		if (rc) {
+			pr_err("Failed to clear kickstart rc=%d\n", rc);
+			return rc;
+		}
+
+		/* From here the lpm_workaround will be active */
+		chip->lockup_lpm_wrkarnd = true;
+
+		/* Enable LPM */
+		pm8921_chg_set_lpm(chip, 1);
+	}
+
+	if (chip->lockup_lpm_wrkarnd) {
+		chip->vreg_xoadc = regulator_get(chip->dev, "vreg_xoadc");
+		if (IS_ERR(chip->vreg_xoadc))
+			return -ENODEV;
+
+		rc = regulator_set_optimum_mode(chip->vreg_xoadc, 10000);
+		if (rc < 0) {
+			pr_err("Failed to set configure HPM rc=%d\n", rc);
+			return rc;
+		}
+
+		rc = regulator_set_voltage(chip->vreg_xoadc, 1800000, 1800000);
+		if (rc) {
+			pr_err("Failed to set L14 voltage rc=%d\n", rc);
+			return rc;
+		}
+
+		rc = regulator_enable(chip->vreg_xoadc);
+		if (rc) {
+			pr_err("Failed to enable L14 rc=%d\n", rc);
+			return rc;
+		}
+	}
+
 	return 0;
 }
 
@@ -4190,9 +4374,9 @@
 	u8 temp;
 
 	temp = (u8) val;
-	ret = pm8xxx_writeb(the_chip->dev->parent, addr, temp);
+	ret = pm_chg_write(the_chip, addr, temp);
 	if (ret) {
-		pr_err("pm8xxx_writeb to %x value =%d errored = %d\n",
+		pr_err("pm_chg_write to %x value =%d errored = %d\n",
 			addr, temp, ret);
 		return -EAGAIN;
 	}
@@ -4378,11 +4562,19 @@
 	int rc;
 	struct pm8921_chg_chip *chip = dev_get_drvdata(dev);
 
+	if (chip->lockup_lpm_wrkarnd) {
+		rc = regulator_disable(chip->vreg_xoadc);
+		if (rc)
+			pr_err("Failed to disable L14 rc=%d\n", rc);
+
+		rc = pm8921_apply_19p2mhz_kickstart(chip);
+		if (rc)
+			pr_err("Failed to apply kickstart rc=%d\n", rc);
+	}
+
 	rc = pm_chg_masked_write(chip, CHG_CNTRL, VREF_BATT_THERM_FORCE_ON, 0);
 	if (rc)
 		pr_err("Failed to Force Vref therm off rc=%d\n", rc);
-	if (!(chip->disable_hw_clock_switching))
-		pm8921_chg_set_hw_clk_switching(chip);
 	return 0;
 }
 
@@ -4391,7 +4583,15 @@
 	int rc;
 	struct pm8921_chg_chip *chip = dev_get_drvdata(dev);
 
-	pm8921_chg_force_19p2mhz_clk(chip);
+	if (chip->lockup_lpm_wrkarnd) {
+		rc = regulator_enable(chip->vreg_xoadc);
+		if (rc)
+			pr_err("Failed to enable L14 rc=%d\n", rc);
+
+		rc = pm8921_apply_19p2mhz_kickstart(chip);
+		if (rc)
+			pr_err("Failed to apply kickstart rc=%d\n", rc);
+	}
 
 	rc = pm_chg_masked_write(chip, CHG_CNTRL, VREF_BATT_THERM_FORCE_ON,
 						VREF_BATT_THERM_FORCE_ON);
@@ -4413,7 +4613,6 @@
 
 static int pm8921_charger_suspend(struct device *dev)
 {
-	int rc;
 	struct pm8921_chg_chip *chip = dev_get_drvdata(dev);
 
 	if (is_usb_chg_plugged_in(chip)) {
@@ -4421,9 +4620,6 @@
 		enable_irq_wake(chip->pmic_chg_irq[LOOP_CHANGE_IRQ]);
 	}
 
-	rc = pm8xxx_batt_alarm_enable(PM8XXX_BATT_ALARM_LOWER_COMPARATOR);
-	if (rc < 0)
-		pr_err("Failed to enable lower comparator\n");
 	return 0;
 }
 static int __devinit pm8921_charger_probe(struct platform_device *pdev)
@@ -4494,6 +4690,16 @@
 	chip->led_src_config = pdata->led_src_config;
 	chip->has_dc_supply = pdata->has_dc_supply;
 	chip->battery_less_hardware = pdata->battery_less_hardware;
+	chip->btc_override = pdata->btc_override;
+	if (chip->btc_override) {
+		chip->btc_delay_ms = pdata->btc_delay_ms;
+		chip->btc_override_cold_decidegc
+			= pdata->btc_override_cold_degc * 10;
+		chip->btc_override_hot_decidegc
+			= pdata->btc_override_hot_degc * 10;
+		chip->btc_panic_if_cant_stop_chg
+			= pdata->btc_panic_if_cant_stop_chg;
+	}
 
 	if (chip->battery_less_hardware)
 		charging_disabled = 1;
@@ -4507,6 +4713,9 @@
 		goto free_chip;
 	}
 
+	if (chip->btc_override)
+		pm8921_chg_btc_override_init(chip);
+
 	chip->usb_psy.name = "usb",
 	chip->usb_psy.type = POWER_SUPPLY_TYPE_USB,
 	chip->usb_psy.supplied_to = pm_power_supplied_to,
@@ -4561,6 +4770,7 @@
 	INIT_WORK(&chip->battery_id_valid_work, battery_id_valid);
 
 	INIT_DELAYED_WORK(&chip->update_heartbeat_work, update_heartbeat);
+	INIT_DELAYED_WORK(&chip->btc_override_work, btc_override_worker);
 
 	rc = request_irqs(chip, pdev);
 	if (rc) {
@@ -4573,17 +4783,6 @@
 	enable_irq_wake(chip->pmic_chg_irq[VBATDET_LOW_IRQ]);
 	enable_irq_wake(chip->pmic_chg_irq[FASTCHG_IRQ]);
 
-	rc = pm8921_charger_configure_batt_alarm(chip);
-	if (rc) {
-		pr_err("Couldn't configure battery alarm! rc=%d\n", rc);
-		goto free_irq;
-	}
-
-	rc = pm8921_charger_enable_batt_alarm(chip);
-	if (rc) {
-		pr_err("Couldn't enable battery alarm! rc=%d\n", rc);
-		goto free_irq;
-	}
 	create_debugfs_entries(chip);
 
 	/* determine what state the charger is in */
@@ -4595,9 +4794,8 @@
 							(chip->update_time)));
 	return 0;
 
-free_irq:
-	free_irqs(chip);
 unregister_batt:
+	wake_lock_destroy(&chip->eoc_wake_lock);
 	power_supply_unregister(&chip->batt_psy);
 unregister_dc:
 	power_supply_unregister(&chip->dc_psy);
@@ -4612,6 +4810,7 @@
 {
 	struct pm8921_chg_chip *chip = platform_get_drvdata(pdev);
 
+	regulator_put(chip->vreg_xoadc);
 	free_irqs(chip);
 	platform_set_drvdata(pdev, NULL);
 	the_chip = NULL;
diff --git a/drivers/power/qpnp-bms.c b/drivers/power/qpnp-bms.c
index 12b186d..51e4465 100644
--- a/drivers/power/qpnp-bms.c
+++ b/drivers/power/qpnp-bms.c
@@ -33,7 +33,7 @@
 #define BMS1_MODE_CTL			0X40
 /* Coulomb counter clear registers */
 #define BMS1_CC_DATA_CTL		0x42
-#define BMS1_CC_CLEAR_CTRL		0x43
+#define BMS1_CC_CLEAR_CTL		0x43
 /* OCV limit registers */
 #define BMS1_OCV_USE_LOW_LIMIT_THR0	0x48
 #define BMS1_OCV_USE_LOW_LIMIT_THR1	0x49
@@ -73,6 +73,8 @@
 #define IAVG_STORAGE_REG		0xB1
 #define SOC_STORAGE_REG			0xB2
 #define BMS1_BMS_DATA_REG_3		0xB3
+/* IADC Channel Select */
+#define IADC1_BMS_ADC_CH_SEL_CTL	0x48
 
 /* Configuration for saving of shutdown soc/iavg */
 #define IGNORE_SOC_TEMP_DECIDEG		50
@@ -105,6 +107,7 @@
 	struct power_supply		*batt_psy;
 	struct spmi_device		*spmi;
 	u16				base;
+	u16				iadc_base;
 
 	u8				revision1;
 	u8				revision2;
@@ -134,6 +137,8 @@
 	struct mutex			last_ocv_uv_mutex;
 	struct mutex			soc_invalidation_mutex;
 
+	bool				use_external_rsense;
+
 	unsigned int			start_percent;
 	unsigned int			end_percent;
 	bool				ignore_shutdown_soc;
@@ -226,29 +231,40 @@
 	return 0;
 }
 
-static int qpnp_masked_write(struct qpnp_bms_chip *chip, u16 addr,
+static int qpnp_masked_write_base(struct qpnp_bms_chip *chip, u16 addr,
 							u8 mask, u8 val)
 {
 	int rc;
 	u8 reg;
 
-	rc = qpnp_read_wrapper(chip, &reg, chip->base + addr, 1);
+	rc = qpnp_read_wrapper(chip, &reg, addr, 1);
 	if (rc) {
-		pr_err("read failed addr = %03X, rc = %d\n",
-				chip->base + addr, rc);
+		pr_err("read failed addr = %03X, rc = %d\n", addr, rc);
 		return rc;
 	}
 	reg &= ~mask;
 	reg |= val & mask;
-	rc = qpnp_write_wrapper(chip, &reg, chip->base + addr, 1);
+	rc = qpnp_write_wrapper(chip, &reg, addr, 1);
 	if (rc) {
 		pr_err("write failed addr = %03X, val = %02x, mask = %02x, reg = %02x, rc = %d\n",
-				chip->base + addr, val, mask, reg, rc);
+					addr, val, mask, reg, rc);
 		return rc;
 	}
 	return 0;
 }
 
+static int qpnp_masked_write_iadc(struct qpnp_bms_chip *chip, u16 addr,
+							u8 mask, u8 val)
+{
+	return qpnp_masked_write_base(chip, chip->iadc_base + addr, mask, val);
+}
+
+static int qpnp_masked_write(struct qpnp_bms_chip *chip, u16 addr,
+							u8 mask, u8 val)
+{
+	return qpnp_masked_write_base(chip, chip->base + addr, mask, val);
+}
+
 #define HOLD_OREG_DATA		BIT(0)
 static int lock_output_data(struct qpnp_bms_chip *chip)
 {
@@ -484,12 +500,49 @@
 	pr_debug("last_good_ocv_uv = %d\n", raw->last_good_ocv_uv);
 }
 
+#define CLEAR_CC			BIT(7)
+#define CLEAR_SW_CC			BIT(6)
+/**
+ * reset both cc and sw-cc.
+ * note: this should only be ever called from one thread
+ * or there may be a race condition where CC is never enabled
+ * again
+ */
+static void reset_cc(struct qpnp_bms_chip *chip)
+{
+	int rc;
+
+	pr_debug("resetting cc manually\n");
+	rc = qpnp_masked_write(chip, BMS1_CC_CLEAR_CTL,
+				CLEAR_CC | CLEAR_SW_CC,
+				CLEAR_CC | CLEAR_SW_CC);
+	if (rc)
+		pr_err("cc reset failed: %d\n", rc);
+
+	/* wait for 100us for cc to reset */
+	udelay(100);
+
+	rc = qpnp_masked_write(chip, BMS1_CC_CLEAR_CTL,
+				CLEAR_CC | CLEAR_SW_CC, 0);
+	if (rc)
+		pr_err("cc reenable failed: %d\n", rc);
+}
+
 static int read_soc_params_raw(struct qpnp_bms_chip *chip,
 				struct raw_soc_params *raw)
 {
 	int rc;
 
 	mutex_lock(&chip->bms_output_lock);
+
+	if (chip->prev_last_good_ocv_raw == 0) {
+		/* software workaround for BMS 1.0
+		 * The coulomb counter does not reset upon PON, so reset it
+		 * manually upon probe. */
+		if (chip->revision1 == 0 && chip->revision2 == 0)
+			reset_cc(chip);
+	}
+
 	lock_output_data(chip);
 
 	rc = qpnp_read_wrapper(chip, (u8 *)&raw->last_good_ocv_raw,
@@ -1030,11 +1083,8 @@
 	return 0;
 }
 
-static int get_simultaneous_batt_v_and_i(
-						struct qpnp_bms_chip *chip,
-						int *ibat_ua, int *vbat_uv)
+static bool is_batfet_open(struct qpnp_bms_chip *chip)
 {
-	int rc;
 	union power_supply_propval ret = {0,};
 
 	if (chip->batt_psy == NULL)
@@ -1043,12 +1093,20 @@
 		/* if battery has been registered, use the status property */
 		chip->batt_psy->get_property(chip->batt_psy,
 					POWER_SUPPLY_PROP_STATUS, &ret);
-	} else {
-		/* default to using separate vbat/ibat if unregistered */
-		ret.intval = POWER_SUPPLY_STATUS_FULL;
+		return ret.intval == POWER_SUPPLY_STATUS_FULL;
 	}
 
-	if (ret.intval == POWER_SUPPLY_STATUS_FULL) {
+	/* Default to true if the battery power supply is not registered. */
+	pr_debug("battery power supply is not registered\n");
+	return true;
+}
+
+static int get_simultaneous_batt_v_and_i(struct qpnp_bms_chip *chip,
+					int *ibat_ua, int *vbat_uv)
+{
+	int rc;
+
+	if (is_batfet_open(chip)) {
 		pr_debug("batfet is open using separate vbat and ibat meas\n");
 		rc = get_battery_voltage(vbat_uv);
 		if (rc < 0) {
@@ -1160,7 +1218,7 @@
 				(s64)params->fcc_uah - params->uuc_uah);
 	soc_est = bound_soc(soc_est);
 
-	if (ibat_ua < 0) {
+	if (ibat_ua < 0 && !is_batfet_open(chip)) {
 		soc = charging_adjustments(chip, params, soc, vbat_uv, ibat_ua,
 				batt_temp);
 		goto out;
@@ -1298,7 +1356,7 @@
 
 	pr_debug("RUC = %duAh\n", remaining_usable_charge_uah);
 	if (params.fcc_uah - params.uuc_uah <= 0) {
-		pr_warn("FCC = %duAh, UUC = %duAh forcing soc = 0\n",
+		pr_debug("FCC = %duAh, UUC = %duAh forcing soc = 0\n",
 						params.fcc_uah,
 						params.uuc_uah);
 		soc = 0;
@@ -1332,12 +1390,12 @@
 		soc = 100;
 
 	if (soc < 0) {
-		pr_err("bad rem_usb_chg = %d rem_chg %d, cc_uah %d, unusb_chg %d\n",
+		pr_debug("bad rem_usb_chg = %d rem_chg %d, cc_uah %d, unusb_chg %d\n",
 				remaining_usable_charge_uah,
 				params.ocv_charge_uah,
 				params.cc_uah, params.uuc_uah);
 
-		pr_err("for bad rem_usb_chg last_ocv_uv = %d batt_temp = %d fcc = %d soc =%d\n",
+		pr_debug("for bad rem_usb_chg last_ocv_uv = %d batt_temp = %d fcc = %d soc =%d\n",
 				chip->last_ocv_uv, batt_temp,
 				params.fcc_uah, soc);
 		soc = 0;
@@ -1622,7 +1680,8 @@
 	}
 
 	/* last_soc < soc  ... scale and catch up */
-	if (chip->last_soc != -EINVAL && chip->last_soc < soc && soc != 100)
+	if (chip->last_soc != -EINVAL && chip->last_soc < soc
+			&& soc != 100 && chip->catch_up_time_us != 0)
 		soc = scale_soc_while_chg(chip, delta_time_us,
 						soc, chip->last_soc);
 
@@ -1902,6 +1961,9 @@
 	SPMI_PROP_READ(low_soc_calculate_soc_ms,
 			"low-soc-calculate-soc-ms", rc);
 	SPMI_PROP_READ(calculate_soc_ms, "calculate-soc-ms", rc);
+	chip->use_external_rsense = of_property_read_bool(
+			chip->spmi->dev.of_node,
+			"qcom,bms-use-external-rsense");
 	chip->ignore_shutdown_soc = of_property_read_bool(
 			chip->spmi->dev.of_node,
 			"qcom,bms-ignore-shutdown-soc");
@@ -1922,7 +1984,7 @@
 			chip->batt_type);
 	pr_debug("ignore_shutdown_soc:%d, use_voltage_soc:%d\n",
 			chip->ignore_shutdown_soc, chip->use_voltage_soc);
-
+	pr_debug("use external rsense: %d\n", chip->use_external_rsense);
 	return 0;
 }
 
@@ -1939,11 +2001,130 @@
 	chip->first_time_calc_uuc = 1;
 }
 
-static int __devinit
-qpnp_bms_probe(struct spmi_device *spmi)
+#define REG_OFFSET_PERP_TYPE			0x04
+#define REG_OFFSET_PERP_SUBTYPE			0x05
+#define BMS_BMS_TYPE				0xD
+#define BMS_BMS_SUBTYPE				0x1
+#define BMS_IADC_TYPE				0x8
+#define BMS_IADC_SUBTYPE			0x3
+
+static int register_spmi(struct qpnp_bms_chip *chip, struct spmi_device *spmi)
+{
+	struct spmi_resource *spmi_resource;
+	struct resource *resource;
+	int rc;
+	u8 type, subtype;
+
+	chip->dev = &(spmi->dev);
+	chip->spmi = spmi;
+
+	spmi_for_each_container_dev(spmi_resource, spmi) {
+		if (!spmi_resource) {
+			pr_err("qpnp_bms: spmi resource absent\n");
+			return -ENXIO;
+		}
+
+		resource = spmi_get_resource(spmi, spmi_resource,
+						IORESOURCE_MEM, 0);
+		if (!(resource && resource->start)) {
+			pr_err("node %s IO resource absent!\n",
+				spmi->dev.of_node->full_name);
+			return -ENXIO;
+		}
+
+		rc = qpnp_read_wrapper(chip, &type,
+				resource->start + REG_OFFSET_PERP_TYPE, 1);
+		if (rc) {
+			pr_err("Peripheral type read failed rc=%d\n", rc);
+			return rc;
+		}
+		rc = qpnp_read_wrapper(chip, &subtype,
+				resource->start + REG_OFFSET_PERP_SUBTYPE, 1);
+		if (rc) {
+			pr_err("Peripheral subtype read failed rc=%d\n", rc);
+			return rc;
+		}
+
+		if (type == BMS_BMS_TYPE && subtype == BMS_BMS_SUBTYPE) {
+			chip->base = resource->start;
+		} else if (type == BMS_IADC_TYPE
+					&& subtype == BMS_IADC_SUBTYPE) {
+			chip->iadc_base = resource->start;
+		} else {
+			pr_err("Invalid peripheral start=0x%x type=0x%x, subtype=0x%x\n",
+					resource->start, type, subtype);
+		}
+	}
+
+	if (chip->base == 0) {
+		dev_err(&spmi->dev, "BMS peripheral was not registered\n");
+		return -EINVAL;
+	}
+	if (chip->iadc_base == 0) {
+		dev_err(&spmi->dev, "BMS_IADC peripheral was not registered\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+#define ADC_CH_SEL_MASK			0x7
+static int read_iadc_channel_select(struct qpnp_bms_chip *chip)
+{
+	u8 iadc_channel_select;
+	int rc;
+
+	rc = qpnp_read_wrapper(chip, &iadc_channel_select,
+			chip->iadc_base + IADC1_BMS_ADC_CH_SEL_CTL, 1);
+	if (rc) {
+		pr_err("Error reading bms_iadc channel register %d\n", rc);
+		return rc;
+	}
+
+	iadc_channel_select &= ADC_CH_SEL_MASK;
+	if (iadc_channel_select == INTERNAL_RSENSE) {
+		pr_debug("Internal rsense used\n");
+		if (chip->use_external_rsense) {
+			pr_debug("Changing rsense to external\n");
+			rc = qpnp_masked_write_iadc(chip,
+					IADC1_BMS_ADC_CH_SEL_CTL,
+					ADC_CH_SEL_MASK,
+					EXTERNAL_RSENSE);
+			if (rc) {
+				pr_err("Unable to set IADC1_BMS channel %x to %x: %d\n",
+						IADC1_BMS_ADC_CH_SEL_CTL,
+						EXTERNAL_RSENSE, rc);
+				return rc;
+			}
+			reset_cc(chip);
+		}
+	} else if (iadc_channel_select == EXTERNAL_RSENSE) {
+		pr_debug("External rsense used\n");
+		if (!chip->use_external_rsense) {
+			pr_debug("Changing rsense to internal\n");
+			rc = qpnp_masked_write_iadc(chip,
+					IADC1_BMS_ADC_CH_SEL_CTL,
+					ADC_CH_SEL_MASK,
+					INTERNAL_RSENSE);
+			if (rc) {
+				pr_err("Unable to set IADC1_BMS channel %x to %x: %d\n",
+						IADC1_BMS_ADC_CH_SEL_CTL,
+						INTERNAL_RSENSE, rc);
+				return rc;
+			}
+			reset_cc(chip);
+		}
+	} else {
+		pr_err("IADC1_BMS_IADC configured incorrectly. Selected channel = %d\n",
+							iadc_channel_select);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+static int __devinit qpnp_bms_probe(struct spmi_device *spmi)
 {
 	struct qpnp_bms_chip *chip;
-	struct resource *bms_resource;
 	int rc, vbatt;
 
 	chip = kzalloc(sizeof *chip, GFP_KERNEL);
@@ -1953,34 +2134,6 @@
 		return -ENOMEM;
 	}
 
-	chip->dev = &(spmi->dev);
-	chip->spmi = spmi;
-
-	mutex_init(&chip->bms_output_lock);
-	mutex_init(&chip->last_ocv_uv_mutex);
-	mutex_init(&chip->soc_invalidation_mutex);
-
-	bms_resource = spmi_get_resource(spmi, NULL, IORESOURCE_MEM, 0);
-	if (!bms_resource) {
-		dev_err(&spmi->dev, "Unable to get BMS base address\n");
-		return -ENXIO;
-	}
-	chip->base = bms_resource->start;
-
-	rc = qpnp_read_wrapper(chip, &chip->revision1,
-			chip->base + BMS1_REVISION1, 1);
-	if (rc) {
-		pr_err("error reading version register %d\n", rc);
-		goto error_read;
-	}
-
-	rc = qpnp_read_wrapper(chip, &chip->revision2,
-			chip->base + BMS1_REVISION2, 1);
-	if (rc) {
-		pr_err("Error reading version register %d\n", rc);
-		goto error_read;
-	}
-
 	rc = qpnp_vadc_is_ready();
 	if (rc) {
 		pr_info("vadc not ready: %d, deferring probe\n", rc);
@@ -1993,20 +2146,51 @@
 		goto error_read;
 	}
 
-	rc = set_battery_data(chip);
+	rc = register_spmi(chip, spmi);
 	if (rc) {
-		pr_err("Bad battery data %d\n", rc);
+		pr_err("error registering spmi resource %d\n", rc);
+		goto error_resource;
+	}
+
+	rc = qpnp_read_wrapper(chip, &chip->revision1,
+			chip->base + BMS1_REVISION1, 1);
+	if (rc) {
+		pr_err("error reading version register %d\n", rc);
 		goto error_read;
 	}
 
+	rc = qpnp_read_wrapper(chip, &chip->revision2,
+			chip->base + BMS1_REVISION2, 1);
+	if (rc) {
+		pr_err("Error reading version register %d\n", rc);
+		goto error_read;
+	}
+	pr_debug("BMS version: %hhu.%hhu\n", chip->revision2, chip->revision1);
+
 	rc = bms_read_properties(chip);
 	if (rc) {
 		pr_err("Unable to read all bms properties, rc = %d\n", rc);
 		goto error_read;
 	}
 
+	rc = read_iadc_channel_select(chip);
+	if (rc) {
+		pr_err("Unable to get iadc selected channel = %d\n", rc);
+		goto error_read;
+	}
+
+	rc = set_battery_data(chip);
+	if (rc) {
+		pr_err("Bad battery data %d\n", rc);
+		goto error_read;
+	}
+
 	bms_initialize_constants(chip);
 
+	mutex_init(&chip->bms_output_lock);
+	mutex_init(&chip->last_ocv_uv_mutex);
+	mutex_init(&chip->soc_invalidation_mutex);
+
 	INIT_DELAYED_WORK(&chip->calculate_soc_delayed_work,
 			calculate_soc_work);
 
@@ -2048,6 +2232,7 @@
 unregister_dc:
 	power_supply_unregister(&chip->bms_psy);
 	dev_set_drvdata(&spmi->dev, NULL);
+error_resource:
 error_read:
 	kfree(chip);
 	return rc;
diff --git a/drivers/power/qpnp-charger.c b/drivers/power/qpnp-charger.c
index b5559db..005f789 100644
--- a/drivers/power/qpnp-charger.c
+++ b/drivers/power/qpnp-charger.c
@@ -71,6 +71,7 @@
 #define CHGR_CHG_WDOG_EN			0x65
 #define CHGR_USB_IUSB_MAX			0x44
 #define CHGR_USB_USB_SUSP			0x47
+#define CHGR_USB_USB_OTG_CTL			0x48
 #define CHGR_USB_ENUM_T_STOP			0x4E
 #define CHGR_CHG_TEMP_THRESH			0x66
 #define CHGR_BAT_IF_PRES_STATUS			0x08
@@ -160,9 +161,12 @@
  * @chg_done:			indicates that charging is completed
  * @usb_present:		present status of usb
  * @dc_present:			present status of dc
+ * @use_default_batt_values:	flag to report default battery properties
  * @max_voltage_mv:		the max volts the batt should be charged up to
- * @min_voltage_mv:		the min battery voltage before turning the FETon
+ * @min_voltage_mv:		min battery voltage before turning the FET on
+ * @resume_voltage_mv:		voltage at which the battery resumes charging
  * @term_current:		the charging based term current
+ * @safe_current:		battery safety current setting
  * @revision:			PMIC revision
  * @dc_psy			power supply to export information to userspace
  * @usb_psy			power supply to export information to userspace
@@ -190,11 +194,14 @@
 	bool				usb_present;
 	bool				dc_present;
 	bool				charging_disabled;
+	bool				use_default_batt_values;
 	unsigned int			max_bat_chg_current;
 	unsigned int			safe_voltage_mv;
 	unsigned int			max_voltage_mv;
 	unsigned int			min_voltage_mv;
+	unsigned int			resume_voltage_mv;
 	unsigned int			term_current;
+	unsigned int			safe_current;
 	unsigned int			revision;
 	struct power_supply		dc_psy;
 	struct power_supply		*usb_psy;
@@ -271,6 +278,27 @@
 	return 0;
 }
 
+#define USB_OTG_EN_BIT	BIT(0)
+static int
+qpnp_chg_is_otg_en_set(struct qpnp_chg_chip *chip)
+{
+	u8 usb_otg_en;
+	int rc;
+
+	rc = qpnp_chg_read(chip, &usb_otg_en,
+				 chip->usb_chgpth_base + CHGR_USB_USB_OTG_CTL,
+				 1);
+
+	if (rc) {
+		pr_err("spmi read failed: addr=%03X, rc=%d\n",
+				chip->usb_chgpth_base + CHGR_STATUS, rc);
+		return rc;
+	}
+	pr_debug("usb otg en 0x%x\n", usb_otg_en);
+
+	return (usb_otg_en & USB_OTG_EN_BIT) ? 1 : 0;
+}
+
 #define USB_VALID_BIT	BIT(7)
 static int
 qpnp_chg_is_usb_chg_plugged_in(struct qpnp_chg_chip *chip)
@@ -380,10 +408,16 @@
 qpnp_chg_usb_usbin_valid_irq_handler(int irq, void *_chip)
 {
 	struct qpnp_chg_chip *chip = _chip;
-	int usb_present;
+	int usb_present, host_mode;
 
 	usb_present = qpnp_chg_is_usb_chg_plugged_in(chip);
-	pr_debug("usbin-valid triggered: %d\n", usb_present);
+	host_mode = qpnp_chg_is_otg_en_set(chip);
+	pr_debug("usbin-valid triggered: %d host_mode: %d\n",
+		usb_present, host_mode);
+
+	/* In host mode notifications cmoe from USB supply */
+	if (host_mode)
+		return IRQ_HANDLED;
 
 	if (chip->usb_present ^ usb_present) {
 		chip->usb_present = usb_present;
@@ -436,6 +470,80 @@
 	return 0;
 }
 
+static int
+qpnp_chg_charge_en(struct qpnp_chg_chip *chip, int enable)
+{
+	return qpnp_chg_masked_write(chip, chip->chgr_base + CHGR_CHG_CTRL,
+			CHGR_CHG_EN,
+			enable ? CHGR_CHG_EN : 0, 1);
+}
+
+static int
+qpnp_chg_force_run_on_batt(struct qpnp_chg_chip *chip, int disable)
+{
+	/* This bit forces the charger to run off of the battery rather
+	 * than a connected charger */
+	return qpnp_chg_masked_write(chip, chip->chgr_base + CHGR_CHG_CTRL,
+			CHGR_ON_BAT_FORCE_BIT,
+			disable ? CHGR_ON_BAT_FORCE_BIT : 0, 1);
+}
+
+static
+int switch_usb_to_charge_mode(struct qpnp_chg_chip *chip)
+{
+	int rc;
+
+	pr_debug("switch to charge mode\n");
+	if (!qpnp_chg_is_otg_en_set(chip))
+		return 0;
+
+	/* enable usb ovp fet */
+	rc = qpnp_chg_masked_write(chip,
+			chip->usb_chgpth_base + CHGR_USB_USB_OTG_CTL,
+			USB_OTG_EN_BIT,
+			0, 1);
+	if (rc) {
+		pr_err("Failed to turn on usb ovp rc = %d\n", rc);
+		return rc;
+	}
+
+	rc = qpnp_chg_force_run_on_batt(chip, chip->charging_disabled);
+	if (rc) {
+		pr_err("Failed re-enable charging rc = %d\n", rc);
+		return rc;
+	}
+
+	return 0;
+}
+
+static
+int switch_usb_to_host_mode(struct qpnp_chg_chip *chip)
+{
+	int rc;
+
+	pr_debug("switch to host mode\n");
+	if (qpnp_chg_is_otg_en_set(chip))
+		return 0;
+
+	rc = qpnp_chg_force_run_on_batt(chip, 1);
+	if (rc) {
+		pr_err("Failed to disable charging rc = %d\n", rc);
+		return rc;
+	}
+
+	/* force usb ovp fet off */
+	rc = qpnp_chg_masked_write(chip,
+			chip->usb_chgpth_base + CHGR_USB_USB_OTG_CTL,
+			USB_OTG_EN_BIT,
+			USB_OTG_EN_BIT, 1);
+	if (rc) {
+		pr_err("Failed to turn off usb ovp rc = %d\n", rc);
+		return rc;
+	}
+
+	return 0;
+}
+
 static enum power_supply_property pm_power_props_mains[] = {
 	POWER_SUPPLY_PROP_PRESENT,
 	POWER_SUPPLY_PROP_ONLINE,
@@ -632,6 +740,9 @@
 {
 	union power_supply_propval ret = {0,};
 
+	if (chip->use_default_batt_values || !get_prop_batt_present(chip))
+		return DEFAULT_CAPACITY;
+
 	if (chip->bms_psy) {
 		chip->bms_psy->get_property(chip->bms_psy,
 			  POWER_SUPPLY_PROP_CAPACITY, &ret);
@@ -645,7 +756,7 @@
 	return DEFAULT_CAPACITY;
 }
 
-#define DEFAULT_TEMP		25
+#define DEFAULT_TEMP		250
 #define MAX_TOLERABLE_BATT_TEMP_DDC	680
 static int
 get_prop_batt_temp(struct qpnp_chg_chip *chip)
@@ -653,6 +764,9 @@
 	int rc = 0;
 	struct qpnp_vadc_result results;
 
+	if (chip->use_default_batt_values || !get_prop_batt_present(chip))
+		return DEFAULT_TEMP;
+
 	if (chip->revision > 0) {
 		rc = qpnp_vadc_read(LR_MUX1_BATT_THERM, &results);
 		if (rc) {
@@ -682,6 +796,21 @@
 		chip->bms_psy = power_supply_get_by_name("bms");
 
 	chip->usb_psy->get_property(chip->usb_psy,
+			  POWER_SUPPLY_PROP_SCOPE, &ret);
+	if (ret.intval) {
+		if ((ret.intval == POWER_SUPPLY_SCOPE_SYSTEM)
+				&& !qpnp_chg_is_otg_en_set(chip)) {
+			switch_usb_to_host_mode(chip);
+			return;
+		}
+		if ((ret.intval == POWER_SUPPLY_SCOPE_DEVICE)
+				&& qpnp_chg_is_otg_en_set(chip)) {
+			switch_usb_to_charge_mode(chip);
+			return;
+		}
+	}
+
+	chip->usb_psy->get_property(chip->usb_psy,
 			  POWER_SUPPLY_PROP_ONLINE, &ret);
 
 	if (ret.intval && qpnp_chg_is_usb_chg_plugged_in(chip)) {
@@ -702,24 +831,6 @@
 }
 
 static int
-qpnp_chg_force_run_on_batt(struct qpnp_chg_chip *chip, int disable)
-{
-	/* This bit forces the charger to run off of the battery rather
-	 * than a connected charger */
-	return qpnp_chg_masked_write(chip, chip->chgr_base + CHGR_CHG_CTRL,
-			CHGR_ON_BAT_FORCE_BIT,
-			disable ? CHGR_ON_BAT_FORCE_BIT : 0, 1);
-}
-
-static int
-qpnp_chg_charge_en(struct qpnp_chg_chip *chip, int enable)
-{
-	return qpnp_chg_masked_write(chip, chip->chgr_base + CHGR_CHG_CTRL,
-			CHGR_CHG_EN,
-			enable ? CHGR_CHG_EN : 0, 1);
-}
-
-static int
 qpnp_batt_power_get_property(struct power_supply *psy,
 				       enum power_supply_property psp,
 				       union power_supply_propval *val)
@@ -828,6 +939,28 @@
 			QPNP_CHG_VINMIN_MASK, temp, 1);
 }
 
+#define QPNP_CHG_IBATSAFE_MIN_MA		100
+#define QPNP_CHG_IBATSAFE_MAX_MA		3250
+#define QPNP_CHG_I_STEP_MA		50
+#define QPNP_CHG_I_MIN_MA		100
+#define QPNP_CHG_I_MASK			0x3F
+static int
+qpnp_chg_ibatsafe_set(struct qpnp_chg_chip *chip, int safe_current)
+{
+	u8 temp;
+
+	if (safe_current < QPNP_CHG_IBATSAFE_MIN_MA
+			|| safe_current > QPNP_CHG_IBATSAFE_MAX_MA) {
+		pr_err("bad mA=%d asked to set\n", safe_current);
+		return -EINVAL;
+	}
+
+	temp = (safe_current - QPNP_CHG_IBATSAFE_MIN_MA)
+				/ QPNP_CHG_I_STEP_MA;
+	return qpnp_chg_masked_write(chip,
+			chip->chgr_base + CHGR_IBAT_SAFE,
+			QPNP_CHG_I_MASK, temp, 1);
+}
 
 #define QPNP_CHG_ITERM_MIN_MA		100
 #define QPNP_CHG_ITERM_MAX_MA		250
@@ -853,9 +986,6 @@
 
 #define QPNP_CHG_IBATMAX_MIN	100
 #define QPNP_CHG_IBATMAX_MAX	3250
-#define QPNP_CHG_I_STEP_MA		50
-#define QPNP_CHG_I_MIN_MA		100
-#define QPNP_CHG_I_MASK			0x3F
 static int
 qpnp_chg_ibatmax_set(struct qpnp_chg_chip *chip, int chg_current)
 {
@@ -871,6 +1001,26 @@
 			QPNP_CHG_I_MASK, temp, 1);
 }
 
+#define QPNP_CHG_VBATDET_MIN_MV	3240
+#define QPNP_CHG_VBATDET_MAX_MV	5780
+#define QPNP_CHG_VBATDET_STEP_MV	20
+static int
+qpnp_chg_vbatdet_set(struct qpnp_chg_chip *chip, int vbatdet_mv)
+{
+	u8 temp;
+
+	if (vbatdet_mv < QPNP_CHG_VBATDET_MIN_MV
+			|| vbatdet_mv > QPNP_CHG_VBATDET_MAX_MV) {
+		pr_err("bad mV=%d asked to set\n", vbatdet_mv);
+		return -EINVAL;
+	}
+	temp = (vbatdet_mv - QPNP_CHG_VBATDET_MIN_MV)
+			/ QPNP_CHG_VBATDET_STEP_MV;
+
+	pr_debug("voltage=%d setting %02x\n", vbatdet_mv, temp);
+	return qpnp_chg_write(chip, &temp,
+		chip->chgr_base + CHGR_VBAT_DET, 1);
+}
 
 #define QPNP_CHG_V_MIN_MV	3240
 #define QPNP_CHG_V_MAX_MV	4500
@@ -973,6 +1123,11 @@
 			pr_debug("failed setting safe_voltage rc=%d\n", rc);
 			return rc;
 		}
+		rc = qpnp_chg_vbatdet_set(chip, chip->resume_voltage_mv);
+		if (rc) {
+			pr_debug("failed setting resume_voltage rc=%d\n", rc);
+			return rc;
+		}
 		rc = qpnp_chg_ibatmax_set(chip, chip->max_bat_chg_current);
 		if (rc) {
 			pr_debug("failed setting ibatmax rc=%d\n", rc);
@@ -983,6 +1138,11 @@
 			pr_debug("failed setting ibatterm rc=%d\n", rc);
 			return rc;
 		}
+		rc = qpnp_chg_ibatsafe_set(chip, chip->safe_current);
+		if (rc) {
+			pr_debug("failed setting ibat_Safe rc=%d\n", rc);
+			return rc;
+		}
 		/* HACK: Disable wdog */
 		rc = qpnp_chg_masked_write(chip, chip->chgr_base + 0x62,
 			0xFF, 0xA0, 1);
@@ -1119,6 +1279,24 @@
 		goto fail_chg_enable;
 	}
 
+	/* Get the ibatsafe property */
+	rc = of_property_read_u32(spmi->dev.of_node,
+				"qcom,chg-vbatdet-mv",
+				&chip->resume_voltage_mv);
+	if (rc) {
+		pr_err("Error reading vbatdet property %d\n", rc);
+		goto fail_chg_enable;
+	}
+
+	/* Get the ibatsafe property */
+	rc = of_property_read_u32(spmi->dev.of_node,
+				"qcom,chg-ibatsafe-ma",
+				&chip->safe_current);
+	if (rc) {
+		pr_err("Error reading ibatsafe property %d\n", rc);
+		goto fail_chg_enable;
+	}
+
 	/* Get the ibatterm property */
 	rc = of_property_read_u32(spmi->dev.of_node,
 				"qcom,chg-ibatterm-ma",
@@ -1140,6 +1318,14 @@
 	chip->charging_disabled = of_property_read_bool(spmi->dev.of_node,
 					"qcom,chg-charging-disabled");
 
+	/* Get the fake-batt-values property */
+	chip->use_default_batt_values = of_property_read_bool(spmi->dev.of_node,
+					"qcom,chg-use-default-batt-values");
+
+	/* Disable charging when faking battery values */
+	if (chip->use_default_batt_values)
+		chip->charging_disabled = true;
+
 	spmi_for_each_container_dev(spmi_resource, spmi) {
 		if (!spmi_resource) {
 			pr_err("qpnp_chg: spmi resource absent\n");
diff --git a/drivers/remoteproc/remoteproc_core.c b/drivers/remoteproc/remoteproc_core.c
index e756a0d..289293a 100644
--- a/drivers/remoteproc/remoteproc_core.c
+++ b/drivers/remoteproc/remoteproc_core.c
@@ -78,7 +78,7 @@
  * the recovery of the remote processor.
  */
 static int rproc_iommu_fault(struct iommu_domain *domain, struct device *dev,
-		unsigned long iova, int flags)
+		unsigned long iova, int flags, void *token)
 {
 	dev_err(dev, "iommu fault: da 0x%lx flags 0x%x\n", iova, flags);
 
@@ -117,7 +117,7 @@
 		return -ENOMEM;
 	}
 
-	iommu_set_fault_handler(domain, rproc_iommu_fault);
+	iommu_set_fault_handler(domain, rproc_iommu_fault, rproc);
 
 	ret = iommu_attach_device(domain, dev);
 	if (ret) {
diff --git a/drivers/spi/spi_qsd.c b/drivers/spi/spi_qsd.c
index dba02f8..6135e71 100644
--- a/drivers/spi/spi_qsd.c
+++ b/drivers/spi/spi_qsd.c
@@ -127,13 +127,72 @@
 	}
 }
 
+/**
+ * msm_spi_clk_max_rate: finds the nearest lower rate for a clk
+ * @clk the clock for which to find nearest lower rate
+ * @rate clock frequency in Hz
+ * @return nearest lower rate or negative error value
+ *
+ * Public clock API extends clk_round_rate which is a ceiling function. This
+ * function is a floor function implemented as a binary search using the
+ * ceiling function.
+ */
+static long msm_spi_clk_max_rate(struct clk *clk, unsigned long rate)
+{
+	long lowest_available, nearest_low, step_size, cur;
+	long step_direction = -1;
+	long guess = rate;
+	int  max_steps = 10;
+
+	cur =  clk_round_rate(clk, rate);
+	if (cur == rate)
+		return rate;
+
+	/* if we got here then: cur > rate */
+	lowest_available =  clk_round_rate(clk, 0);
+	if (lowest_available > rate)
+		return -EINVAL;
+
+	step_size = (rate - lowest_available) >> 1;
+	nearest_low = lowest_available;
+
+	while (max_steps-- && step_size) {
+		guess += step_size * step_direction;
+
+		cur =  clk_round_rate(clk, guess);
+
+		if ((cur < rate) && (cur > nearest_low))
+			nearest_low = cur;
+
+		/*
+		 * if we stepped too far, then start stepping in the other
+		 * direction with half the step size
+		 */
+		if (((cur > rate) && (step_direction > 0))
+		 || ((cur < rate) && (step_direction < 0))) {
+			step_direction = -step_direction;
+			step_size >>= 1;
+		 }
+	}
+	return nearest_low;
+}
+
 static void msm_spi_clock_set(struct msm_spi *dd, int speed)
 {
+	long rate;
 	int rc;
 
-	rc = clk_set_rate(dd->clk, speed);
+	rate = msm_spi_clk_max_rate(dd->clk, speed);
+	if (rate < 0) {
+		dev_err(dd->dev,
+		"%s: no match found for requested clock frequency:%d",
+			__func__, speed);
+		return;
+	}
+
+	rc = clk_set_rate(dd->clk, rate);
 	if (!rc)
-		dd->clock_speed = speed;
+		dd->clock_speed = rate;
 }
 
 static int msm_spi_calculate_size(int *fifo_size,
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index e70924c..347d9f2 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -107,3 +107,13 @@
 	  available. If allowed via compile time configuration; enabling the
 	  thermal zone device via the mode file results in shifting PMIC over
 	  temperature shutdown control from hardware to software.
+
+config THERMAL_QPNP_ADC_TM
+	tristate "Qualcomm 8974 Thermal Monitor ADC Driver"
+	depends on THERMAL && SPMI
+	help
+	  This enables the thermal Sysfs driver for the ADC thermal monitoring
+	  device. It shows up in Sysfs as a thermal zone with multiple trip points.
+	  Disabling the thermal zone device via the mode file results in disabling
+	  the sensor. Also able to set threshold temperature for both hot and cold
+	  and update when a threshold is reached.
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index 3b2b3a8..01dce2d 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -11,3 +11,4 @@
 obj-$(CONFIG_SPEAR_THERMAL)		+= spear_thermal.o
 obj-$(CONFIG_THERMAL_TSENS8974)	+= msm8974-tsens.o
 obj-$(CONFIG_THERMAL_QPNP)	+= qpnp-temp-alarm.o
+obj-$(CONFIG_THERMAL_QPNP_ADC_TM)	+= qpnp-adc-tm.o
diff --git a/drivers/thermal/qpnp-adc-tm.c b/drivers/thermal/qpnp-adc-tm.c
new file mode 100644
index 0000000..bcc85f3
--- /dev/null
+++ b/drivers/thermal/qpnp-adc-tm.c
@@ -0,0 +1,1531 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#define pr_fmt(fmt) "%s: " fmt, __func__
+
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/types.h>
+#include <linux/hwmon.h>
+#include <linux/module.h>
+#include <linux/debugfs.h>
+#include <linux/spmi.h>
+#include <linux/of_irq.h>
+#include <linux/wakelock.h>
+#include <linux/interrupt.h>
+#include <linux/completion.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/qpnp/qpnp-adc.h>
+#include <linux/thermal.h>
+#include <linux/platform_device.h>
+
+/* QPNP VADC TM register definition */
+#define QPNP_STATUS1					0x8
+#define QPNP_STATUS1_OP_MODE				4
+#define QPNP_STATUS1_MEAS_INTERVAL_EN_STS		BIT(2)
+#define QPNP_STATUS1_REQ_STS				BIT(1)
+#define QPNP_STATUS1_EOC				BIT(0)
+#define QPNP_STATUS2					0x9
+#define QPNP_STATUS2_CONV_SEQ_STATE			6
+#define QPNP_STATUS2_FIFO_NOT_EMPTY_FLAG		BIT(1)
+#define QPNP_STATUS2_CONV_SEQ_TIMEOUT_STS		BIT(0)
+#define QPNP_CONV_TIMEOUT_ERR				2
+
+#define QPNP_MODE_CTL					0x40
+#define QPNP_OP_MODE_SHIFT				3
+#define QPNP_VREF_XO_THM_FORCE				BIT(2)
+#define QPNP_AMUX_TRIM_EN				BIT(1)
+#define QPNP_ADC_TRIM_EN				BIT(0)
+#define QPNP_EN_CTL1					0x46
+#define QPNP_ADC_TM_EN					BIT(7)
+#define QPNP_ADC_CH_SEL_CTL				0x48
+#define QPNP_ADC_DIG_PARAM				0x50
+#define QPNP_ADC_DIG_DEC_RATIO_SEL_SHIFT		3
+#define QPNP_HW_SETTLE_DELAY				0x51
+#define QPNP_CONV_REQ					0x52
+#define QPNP_CONV_REQ_SET				BIT(7)
+#define QPNP_CONV_SEQ_CTL				0x54
+#define QPNP_CONV_SEQ_HOLDOFF_SHIFT			4
+#define QPNP_CONV_SEQ_TRIG_CTL				0x55
+#define QPNP_ADC_TM_MEAS_INTERVAL_CTL			0x57
+#define QPNP_ADC_TM_MEAS_INTERVAL_TIME_SHIFT		0x3
+#define QPNP_ADC_TM_MEAS_INTERVAL_CTL2			0x58
+#define QPNP_ADC_TM_MEAS_INTERVAL_CTL2_SHIFT		0x4
+#define QPNP_ADC_TM_MEAS_INTERVAL_CTL2_MASK		0xf0
+#define QPNP_ADC_TM_MEAS_INTERVAL_CTL3_MASK		0xf
+
+#define QPNP_ADC_MEAS_INTERVAL_OP_CTL			0x59
+#define QPNP_ADC_MEAS_INTERVAL_OP			BIT(7)
+
+#define QPNP_FAST_AVG_CTL				0x5a
+#define QPNP_FAST_AVG_EN				0x5b
+
+#define QPNP_M0_LOW_THR_LSB				0x5c
+#define QPNP_M0_LOW_THR_MSB				0x5d
+#define QPNP_M0_HIGH_THR_LSB				0x5e
+#define QPNP_M0_HIGH_THR_MSB				0x5f
+#define QPNP_M1_LOW_THR_LSB				0x69
+#define QPNP_M1_LOW_THR_MSB				0x6a
+#define QPNP_M1_HIGH_THR_LSB				0x6b
+#define QPNP_M1_HIGH_THR_MSB				0x6c
+#define QPNP_M2_LOW_THR_LSB				0x71
+#define QPNP_M2_LOW_THR_MSB				0x72
+#define QPNP_M2_HIGH_THR_LSB				0x7b
+#define QPNP_M2_HIGH_THR_MSB				0x7c
+#define QPNP_M3_LOW_THR_LSB				0x79
+#define QPNP_M3_LOW_THR_MSB				0x7a
+#define QPNP_M3_HIGH_THR_LSB				0x7b
+#define QPNP_M3_HIGH_THR_MSB				0x7c
+#define QPNP_M4_LOW_THR_LSB				0x81
+#define QPNP_M4_LOW_THR_MSB				0x82
+#define QPNP_M4_HIGH_THR_LSB				0x83
+#define QPNP_M4_HIGH_THR_MSB				0x84
+
+#define QPNP_ADC_TM_MULTI_MEAS_EN			0x41
+#define QPNP_ADC_TM_MULTI_MEAS_EN_M0			BIT(0)
+#define QPNP_ADC_TM_MULTI_MEAS_EN_M1			BIT(1)
+#define QPNP_ADC_TM_MULTI_MEAS_EN_M2			BIT(2)
+#define QPNP_ADC_TM_MULTI_MEAS_EN_M3			BIT(3)
+#define QPNP_ADC_TM_MULTI_MEAS_EN_M4			BIT(4)
+#define QPNP_ADC_TM_LOW_THR_INT_EN			0x42
+#define QPNP_ADC_TM_LOW_THR_INT_EN_M0			BIT(0)
+#define QPNP_ADC_TM_LOW_THR_INT_EN_M1			BIT(1)
+#define QPNP_ADC_TM_LOW_THR_INT_EN_M2			BIT(2)
+#define QPNP_ADC_TM_LOW_THR_INT_EN_M3			BIT(3)
+#define QPNP_ADC_TM_LOW_THR_INT_EN_M4			BIT(4)
+#define QPNP_ADC_TM_HIGH_THR_INT_EN			0x43
+#define QPNP_ADC_TM_HIGH_THR_INT_EN_M0			BIT(0)
+#define QPNP_ADC_TM_HIGH_THR_INT_EN_M1			BIT(1)
+#define QPNP_ADC_TM_HIGH_THR_INT_EN_M2			BIT(2)
+#define QPNP_ADC_TM_HIGH_THR_INT_EN_M3			BIT(3)
+#define QPNP_ADC_TM_HIGH_THR_INT_EN_M4			BIT(4)
+
+#define QPNP_ADC_TM_M0_MEAS_INTERVAL_CTL			0x57
+#define QPNP_ADC_TM_M1_MEAS_INTERVAL_CTL			0x6d
+#define QPNP_ADC_TM_M2_MEAS_INTERVAL_CTL			0x75
+#define QPNP_ADC_TM_M3_MEAS_INTERVAL_CTL			0x7d
+#define QPNP_ADC_TM_M4_MEAS_INTERVAL_CTL			0x85
+#define QPNP_ADC_TM_STATUS1				0x8
+#define QPNP_ADC_TM_STATUS_LOW				0xa
+#define QPNP_ADC_TM_STATUS_HIGH				0xb
+
+#define QPNP_ADC_TM_M0_LOW_THR				0x5d5c
+#define QPNP_ADC_TM_M0_HIGH_THR				0x5f5e
+#define QPNP_ADC_TM_MEAS_INTERVAL			0x0
+
+#define QPNP_ADC_TM_THR_LSB_MASK(val)			(val & 0xff)
+#define QPNP_ADC_TM_THR_MSB_MASK(val)			((val & 0xff00) >> 8)
+
+struct qpnp_adc_tm_sensor {
+	struct thermal_zone_device	*tz_dev;
+	enum thermal_device_mode	mode;
+	uint32_t			sensor_num;
+	enum qpnp_adc_meas_timer_select	timer_select;
+	uint32_t			meas_interval;
+	uint32_t			low_thr;
+	uint32_t			high_thr;
+	uint32_t			btm_channel_num;
+	struct work_struct		work;
+};
+
+struct qpnp_adc_tm_drv {
+	struct qpnp_adc_drv		*adc;
+	struct qpnp_adc_tm_usbid_param	*usb_id_param;
+	struct work_struct		usbid_work;
+	struct qpnp_adc_tm_btm_param	*battery_param;
+	struct work_struct		batt_work;
+	bool				adc_tm_initialized;
+	struct qpnp_adc_tm_sensor	sensor[0];
+};
+
+struct qpnp_adc_tm_drv	*qpnp_adc_tm;
+
+struct qpnp_adc_tm_trip_reg_type {
+	uint16_t low_thr_lsb_addr;
+	uint16_t low_thr_msb_addr;
+	uint16_t high_thr_lsb_addr;
+	uint16_t high_thr_msb_addr;
+	u8 multi_meas_en;
+	u8 low_thr_int_chan_en;
+	u8 high_thr_int_chan_en;
+};
+
+static struct qpnp_adc_tm_trip_reg_type adc_tm_data[] = {
+	[QPNP_ADC_TM_M0_ADC_CH_SEL_CTL] = {QPNP_M0_LOW_THR_LSB,
+		QPNP_M0_LOW_THR_MSB, QPNP_M0_HIGH_THR_LSB,
+		QPNP_M0_HIGH_THR_MSB, QPNP_ADC_TM_MULTI_MEAS_EN_M0,
+		QPNP_ADC_TM_LOW_THR_INT_EN_M0, QPNP_ADC_TM_HIGH_THR_INT_EN_M0},
+	[QPNP_ADC_TM_M1_ADC_CH_SEL_CTL] = {QPNP_M1_LOW_THR_LSB,
+		QPNP_M1_LOW_THR_MSB, QPNP_M1_HIGH_THR_LSB,
+		QPNP_M1_HIGH_THR_MSB, QPNP_ADC_TM_MULTI_MEAS_EN_M1,
+		QPNP_ADC_TM_LOW_THR_INT_EN_M1, QPNP_ADC_TM_HIGH_THR_INT_EN_M1},
+	[QPNP_ADC_TM_M2_ADC_CH_SEL_CTL] = {QPNP_M2_LOW_THR_LSB,
+		QPNP_M2_LOW_THR_MSB, QPNP_M2_HIGH_THR_LSB,
+		QPNP_M2_HIGH_THR_MSB, QPNP_ADC_TM_MULTI_MEAS_EN_M2,
+		QPNP_ADC_TM_LOW_THR_INT_EN_M2, QPNP_ADC_TM_HIGH_THR_INT_EN_M2},
+	[QPNP_ADC_TM_M3_ADC_CH_SEL_CTL] = {QPNP_M3_LOW_THR_LSB,
+		QPNP_M3_LOW_THR_MSB, QPNP_M3_HIGH_THR_LSB,
+		QPNP_M3_HIGH_THR_MSB, QPNP_ADC_TM_MULTI_MEAS_EN_M3,
+		QPNP_ADC_TM_LOW_THR_INT_EN_M3, QPNP_ADC_TM_HIGH_THR_INT_EN_M3},
+	[QPNP_ADC_TM_M4_ADC_CH_SEL_CTL] = {QPNP_M4_LOW_THR_LSB,
+		QPNP_M4_LOW_THR_MSB, QPNP_M4_HIGH_THR_LSB,
+		QPNP_M4_HIGH_THR_MSB, QPNP_ADC_TM_MULTI_MEAS_EN_M4,
+		QPNP_ADC_TM_LOW_THR_INT_EN_M4, QPNP_ADC_TM_HIGH_THR_INT_EN_M4},
+};
+
+static int32_t qpnp_adc_tm_read_reg(int16_t reg, u8 *data)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	int rc = 0;
+
+	rc = spmi_ext_register_readl(adc_tm->adc->spmi->ctrl,
+		adc_tm->adc->slave, (adc_tm->adc->offset + reg), data, 1);
+	if (rc < 0)
+		pr_err("adc-tm read reg %d failed with %d\n", reg, rc);
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_write_reg(int16_t reg, u8 data)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	int rc = 0;
+	u8 *buf;
+
+	buf = &data;
+
+	rc = spmi_ext_register_writel(adc_tm->adc->spmi->ctrl,
+		adc_tm->adc->slave, (adc_tm->adc->offset + reg), buf, 1);
+	if (rc < 0)
+		pr_err("adc-tm write reg %d failed with %d\n", reg, rc);
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_enable(bool state)
+{
+	int rc = 0;
+	u8 data = 0, enable_check = 0;
+
+	if (state) {
+		data = QPNP_ADC_TM_EN;
+		rc = qpnp_adc_tm_write_reg(QPNP_EN_CTL1,
+					data);
+		if (rc < 0)
+			pr_err("adc-tm enable failed\n");
+	} else {
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_MULTI_MEAS_EN,
+					&enable_check);
+		if (rc < 0) {
+			pr_err("multi measurement read failed\n");
+			return rc;
+		}
+
+		if (!enable_check) {
+			data = 0;
+			rc = qpnp_adc_tm_write_reg(QPNP_EN_CTL1, data);
+			if (rc < 0)
+				pr_err("adc-tm disable failed\n");
+		}
+	}
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_enable_req_sts_check(void)
+{
+	u8 status1;
+	int rc;
+
+	/* The VADC_TM bank needs to be disabled for new conversion request */
+	rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS1, &status1);
+	if (rc) {
+		pr_err("adc-tm read status1 failed\n");
+		return rc;
+	}
+
+	/* Disable the bank if a conversion is occuring */
+	if (status1 & QPNP_STATUS1_REQ_STS) {
+		rc = qpnp_adc_tm_write_reg(QPNP_EN_CTL1, 0);
+		if (rc < 0)
+			pr_err("adc-tm disable failed\n");
+	}
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_mode_select(u8 mode_ctl)
+{
+	int rc;
+
+	/* VADC_BTM current sets mode to recurring measurements */
+	rc = qpnp_adc_tm_write_reg(QPNP_MODE_CTL, mode_ctl);
+	if (rc < 0)
+		pr_err("adc-tm write mode selection err\n");
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_timer_interval_select(
+		struct qpnp_vadc_chan_properties *chan_prop)
+{
+	int rc;
+	u8 meas_interval_timer2 = 0;
+
+	/* Configure USB_ID to timer1, batt_therm to timer2 */
+	switch (chan_prop->timer_select) {
+	case ADC_MEAS_TIMER_SELECT1:
+		rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_MEAS_INTERVAL_CTL,
+				chan_prop->meas_interval1);
+		if (rc < 0) {
+			pr_err("timer1 configure failed\n");
+			return rc;
+		}
+	break;
+	case ADC_MEAS_TIMER_SELECT2:
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_MEAS_INTERVAL_CTL2,
+				&meas_interval_timer2);
+		if (rc < 0) {
+			pr_err("timer2 configure read failed\n");
+			return rc;
+		}
+		meas_interval_timer2 |=
+			(chan_prop->meas_interval2 <<
+			QPNP_ADC_TM_MEAS_INTERVAL_CTL2_SHIFT);
+		rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_MEAS_INTERVAL_CTL2,
+			meas_interval_timer2);
+		if (rc < 0) {
+			pr_err("timer2 configure failed\n");
+			return rc;
+		}
+	break;
+	case ADC_MEAS_TIMER_SELECT3:
+		/* Thermal channels uses timer3, default to 1 second */
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_MEAS_INTERVAL_CTL2,
+				&meas_interval_timer2);
+		if (rc < 0) {
+			pr_err("timer3 read failed\n");
+			return rc;
+		}
+		chan_prop->meas_interval2 = ADC_MEAS3_INTERVAL_1S;
+		meas_interval_timer2 |= chan_prop->meas_interval2;
+		rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_MEAS_INTERVAL_CTL2,
+			meas_interval_timer2);
+		if (rc < 0) {
+			pr_err("timer3 configure failed\n");
+			return rc;
+		}
+	break;
+	default:
+		pr_err("Invalid timer selection\n");
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_meas_int_update(uint16_t reg_addr_src,
+		u8 reg_addr_dst, bool state)
+{
+	u8 bit_mask_check = 0;
+	int rc = 0;
+
+	rc = qpnp_adc_tm_read_reg(reg_addr_src, &bit_mask_check);
+	if (rc < 0) {
+		pr_err("read failed for addr:%x\n", reg_addr_src);
+		return rc;
+	}
+
+	if (state)
+		bit_mask_check |= reg_addr_dst;
+	else
+		bit_mask_check &= ~reg_addr_dst;
+
+	rc = qpnp_adc_tm_write_reg(reg_addr_src, bit_mask_check);
+	if (rc < 0)
+		pr_err("write failed for addr:%x\n", reg_addr_src);
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_usbid_btm_thr_en(uint32_t btm_chan,
+			struct qpnp_vadc_chan_properties *chan_prop)
+{
+	int rc = 0;
+
+	rc = qpnp_adc_tm_write_reg(
+			adc_tm_data[btm_chan].low_thr_lsb_addr,
+			QPNP_ADC_TM_THR_LSB_MASK(chan_prop->low_thr));
+	if (rc < 0) {
+		pr_err("low threshold lsb setting failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_write_reg(
+		adc_tm_data[btm_chan].low_thr_msb_addr,
+		QPNP_ADC_TM_THR_MSB_MASK(chan_prop->low_thr));
+	if (rc < 0) {
+		pr_err("low threshold msb setting failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_write_reg(
+		adc_tm_data[btm_chan].high_thr_lsb_addr,
+		QPNP_ADC_TM_THR_LSB_MASK(chan_prop->high_thr));
+	if (rc < 0) {
+		pr_err("high threshold lsb setting failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_write_reg(
+		adc_tm_data[btm_chan].high_thr_msb_addr,
+		QPNP_ADC_TM_THR_MSB_MASK(chan_prop->high_thr));
+	if (rc < 0)
+		pr_err("high threshold msb setting failed\n");
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_channel_configure(uint32_t btm_chan,
+			struct qpnp_vadc_chan_properties *chan_prop,
+			uint32_t amux_channel)
+{
+	int rc = 0;
+
+	switch (btm_chan) {
+	case QPNP_ADC_TM_M0_ADC_CH_SEL_CTL:
+	case QPNP_ADC_TM_M1_ADC_CH_SEL_CTL:
+		/* Update low and high notification thresholds */
+		rc = qpnp_adc_tm_usbid_btm_thr_en(btm_chan,
+				chan_prop);
+		if (rc < 0) {
+			pr_err("setting chan:%d threshold failed\n", btm_chan);
+			return rc;
+		}
+
+		if ((chan_prop->state_request ==
+					ADC_TM_LOW_THR_ENABLE) ||
+			(chan_prop->state_request ==
+					ADC_TM_HIGH_LOW_THR_ENABLE)) {
+			/* Enable low threshold's interrupt */
+			rc = qpnp_adc_tm_meas_int_update(
+				QPNP_ADC_TM_LOW_THR_INT_EN,
+				adc_tm_data[btm_chan].low_thr_int_chan_en,
+				true);
+			if (rc < 0) {
+				pr_err("low thr enable err:%d\n", btm_chan);
+				return rc;
+			}
+		}
+
+		if ((chan_prop->state_request ==
+					ADC_TM_HIGH_THR_ENABLE) ||
+			(chan_prop->state_request ==
+					ADC_TM_HIGH_LOW_THR_ENABLE)) {
+			/* Enable high threshold's interrupt */
+			rc = qpnp_adc_tm_meas_int_update(
+				QPNP_ADC_TM_HIGH_THR_INT_EN,
+				adc_tm_data[btm_chan].high_thr_int_chan_en,
+				true);
+			if (rc < 0) {
+				pr_err("high thr enable err:%d\n", btm_chan);
+				return rc;
+			}
+		}
+	/* intention fall through to configure common chan meas */
+	case QPNP_ADC_TM_M2_ADC_CH_SEL_CTL:
+	case QPNP_ADC_TM_M3_ADC_CH_SEL_CTL:
+	case QPNP_ADC_TM_M4_ADC_CH_SEL_CTL:
+		/* Configure AMUX control register for channel selection */
+		rc = qpnp_adc_tm_write_reg(btm_chan, amux_channel);
+		if (rc < 0) {
+			pr_err("btm_chan:%d selection failed\n", btm_chan);
+			return rc;
+		}
+
+		/* Enable corresponding BTM channel measurement */
+		rc = qpnp_adc_tm_meas_int_update(
+			QPNP_ADC_TM_MULTI_MEAS_EN,
+			adc_tm_data[btm_chan].multi_meas_en, true);
+		if (rc < 0) {
+			pr_err("multi measurement en failed\n");
+			return rc;
+		}
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int32_t qpnp_adc_tm_configure(
+			struct qpnp_adc_amux_properties *chan_prop)
+{
+	u8 decimation = 0;
+	int rc = 0;
+	uint32_t btm_chan = 0;
+
+	/* Check if a conversion is in progress */
+	rc = qpnp_adc_tm_enable_req_sts_check();
+	if (rc < 0) {
+		pr_err("adc-tm req_sts check failed\n");
+		return rc;
+	}
+
+	/* Set measurement in recurring mode */
+	rc = qpnp_adc_tm_mode_select(chan_prop->mode_sel);
+	if (rc < 0) {
+		pr_err("adc-tm mode select failed\n");
+		return rc;
+	}
+
+	/* Configure AMUX channel */
+	rc = qpnp_adc_tm_write_reg(QPNP_ADC_CH_SEL_CTL,
+				chan_prop->amux_channel);
+	if (rc < 0) {
+		pr_err("adc-tm channel selection err\n");
+		return rc;
+	}
+
+	/* Digital paramater setup */
+	decimation |= chan_prop->decimation <<
+				QPNP_ADC_DIG_DEC_RATIO_SEL_SHIFT;
+	rc = qpnp_adc_tm_write_reg(QPNP_ADC_DIG_PARAM, decimation);
+	if (rc < 0) {
+		pr_err("adc-tm digital parameter setup err\n");
+		return rc;
+	}
+
+	/* Hardware setting time */
+	rc = qpnp_adc_tm_write_reg(QPNP_HW_SETTLE_DELAY,
+					chan_prop->hw_settle_time);
+	if (rc < 0) {
+		pr_err("adc-tm hw settling time setup err\n");
+		return rc;
+	}
+
+	/* Fast averaging setup */
+	rc = qpnp_adc_tm_write_reg(QPNP_FAST_AVG_CTL,
+					chan_prop->fast_avg_setup);
+	if (rc < 0) {
+		pr_err("adc-tm fast-avg setup err\n");
+		return rc;
+	}
+
+	/* Measurement interval setup */
+	rc = qpnp_adc_tm_timer_interval_select(chan_prop->chan_prop);
+	if (rc < 0) {
+		pr_err("adc-tm timer select failed\n");
+		return rc;
+	}
+
+	/* Channel configuration setup */
+	btm_chan = chan_prop->chan_prop->tm_channel_select;
+	rc = qpnp_adc_tm_channel_configure(btm_chan, chan_prop->chan_prop,
+					chan_prop->amux_channel);
+	if (rc < 0) {
+		pr_err("adc-tm channel configure failed\n");
+		return rc;
+	}
+
+	/* Enable bank */
+	rc = qpnp_adc_tm_enable(true);
+	if (rc)
+		return rc;
+
+	/* Recurring interval measurement enable */
+	rc = qpnp_adc_tm_meas_int_update(QPNP_ADC_MEAS_INTERVAL_OP_CTL,
+			QPNP_ADC_MEAS_INTERVAL_OP, true);
+	if (rc < 0) {
+		pr_err("adc-tm meas interval op configure failed\n");
+		return rc;
+	}
+
+	/* Request conversion */
+	rc = qpnp_adc_tm_write_reg(QPNP_CONV_REQ, QPNP_CONV_REQ_SET);
+	if (rc < 0) {
+		pr_err("adc-tm request conversion failed\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+static int qpnp_adc_tm_get_mode(struct thermal_zone_device *thermal,
+			      enum thermal_device_mode *mode)
+{
+	struct qpnp_adc_tm_sensor *adc_tm = thermal->devdata;
+
+	if (!adc_tm || !mode)
+		return -EINVAL;
+
+	*mode = adc_tm->mode;
+
+	return 0;
+}
+
+static int qpnp_adc_tm_set_mode(struct thermal_zone_device *thermal,
+			      enum thermal_device_mode mode)
+{
+	struct qpnp_adc_tm_sensor *adc_tm = thermal->devdata;
+	struct qpnp_adc_tm_drv *adc_drv = qpnp_adc_tm;
+	int rc = 0, channel;
+
+	if (!adc_tm)
+		return -EINVAL;
+
+	if (mode == THERMAL_DEVICE_ENABLED) {
+		adc_drv->adc->amux_prop->amux_channel = adc_tm->sensor_num;
+		channel = adc_tm->sensor_num;
+		adc_drv->adc->amux_prop->decimation =
+			adc_drv->adc->adc_channels[channel].adc_decimation;
+		adc_drv->adc->amux_prop->hw_settle_time =
+			adc_drv->adc->adc_channels[channel].hw_settle_time;
+		adc_drv->adc->amux_prop->fast_avg_setup =
+			adc_drv->adc->adc_channels[channel].fast_avg_setup;
+		adc_drv->adc->amux_prop->mode_sel =
+			ADC_OP_MEASUREMENT_INTERVAL << QPNP_OP_MODE_SHIFT;
+		adc_drv->adc->amux_prop->chan_prop->timer_select =
+					ADC_MEAS_TIMER_SELECT1;
+		adc_drv->adc->amux_prop->chan_prop->meas_interval1 =
+						ADC_MEAS1_INTERVAL_1S;
+		adc_drv->adc->amux_prop->chan_prop->low_thr = adc_tm->low_thr;
+		adc_drv->adc->amux_prop->chan_prop->high_thr = adc_tm->high_thr;
+		adc_drv->adc->amux_prop->chan_prop->tm_channel_select =
+			adc_tm->btm_channel_num;
+
+		rc = qpnp_adc_tm_configure(adc_drv->adc->amux_prop);
+		if (rc) {
+			pr_err("adc-tm tm configure failed with %d\n", rc);
+			return -EINVAL;
+		}
+	} else if (mode == THERMAL_DEVICE_DISABLED) {
+		rc = qpnp_adc_tm_meas_int_update(QPNP_ADC_TM_MULTI_MEAS_EN,
+			adc_tm_data[adc_tm->btm_channel_num].multi_meas_en,
+			false);
+		if (rc < 0) {
+			pr_err("multi measurement update failed\n");
+			return rc;
+		}
+
+		rc = qpnp_adc_tm_enable(false);
+		if (rc < 0) {
+			pr_err("adc-tm disable failed\n");
+			return rc;
+		}
+	}
+
+	adc_tm->mode = mode;
+
+	return 0;
+}
+
+static int qpnp_adc_tm_get_trip_type(struct thermal_zone_device *thermal,
+				   int trip, enum thermal_trip_type *type)
+{
+	struct qpnp_adc_tm_sensor *adc_tm = thermal->devdata;
+
+	if (!adc_tm || !type || type < 0)
+		return -EINVAL;
+
+	switch (trip) {
+	case ADC_TM_TRIP_HIGH_WARM:
+		*type = THERMAL_TRIP_CONFIGURABLE_HI;
+	break;
+	case ADC_TM_TRIP_LOW_COOL:
+		*type = THERMAL_TRIP_CONFIGURABLE_LOW;
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int qpnp_adc_tm_get_trip_temp(struct thermal_zone_device *thermal,
+				   int trip, unsigned long *temp)
+{
+	struct qpnp_adc_tm_sensor *adc_tm_sensor = thermal->devdata;
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	int64_t result = 0;
+	u8 trip_cool_thr0, trip_cool_thr1, trip_warm_thr0, trip_warm_thr1;
+	unsigned int reg, rc = 0, btm_channel_num;
+	uint16_t reg_low_thr_lsb, reg_low_thr_msb;
+	uint16_t reg_high_thr_lsb, reg_high_thr_msb;
+
+	if (!adc_tm)
+		return -EINVAL;
+
+	btm_channel_num = adc_tm_sensor->btm_channel_num;
+	reg_low_thr_lsb = adc_tm_data[btm_channel_num].low_thr_lsb_addr;
+	reg_low_thr_msb = adc_tm_data[btm_channel_num].low_thr_msb_addr;
+	reg_high_thr_lsb = adc_tm_data[btm_channel_num].high_thr_lsb_addr;
+	reg_high_thr_msb = adc_tm_data[btm_channel_num].high_thr_msb_addr;
+
+	switch (trip) {
+	case ADC_TM_TRIP_HIGH_WARM:
+		rc = qpnp_adc_tm_read_reg(reg_low_thr_lsb, &trip_warm_thr0);
+		if (rc) {
+			pr_err("adc-tm low_thr_lsb err\n");
+			return rc;
+		}
+
+		rc = qpnp_adc_tm_read_reg(reg_low_thr_msb, &trip_warm_thr1);
+		if (rc) {
+			pr_err("adc-tm low_thr_msb err\n");
+			return rc;
+		}
+	reg = (trip_warm_thr1 << 8) | trip_warm_thr0;
+	break;
+	case ADC_TM_TRIP_LOW_COOL:
+		rc = qpnp_adc_tm_read_reg(reg_high_thr_lsb, &trip_cool_thr0);
+		if (rc) {
+			pr_err("adc-tm_tm high_thr_lsb err\n");
+			return rc;
+		}
+
+		rc = qpnp_adc_tm_read_reg(reg_high_thr_msb, &trip_cool_thr1);
+		if (rc) {
+			pr_err("adc-tm_tm high_thr_lsb err\n");
+			return rc;
+		}
+	reg = (trip_cool_thr1 << 8) | trip_cool_thr0;
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	rc = qpnp_adc_tm_scale_voltage_therm_pu2(reg, &result);
+	if (rc < 0) {
+		pr_err("Failed to lookup the therm thresholds\n");
+		return rc;
+	}
+
+	*temp = result;
+
+	return 0;
+}
+
+static int qpnp_adc_tm_set_trip_temp(struct thermal_zone_device *thermal,
+				   int trip, long temp)
+{
+	struct qpnp_adc_tm_sensor *adc_tm_sensor = thermal->devdata;
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	struct qpnp_adc_tm_config tm_config;
+	u8 trip_cool_thr0, trip_cool_thr1, trip_warm_thr0, trip_warm_thr1;
+	uint16_t reg_low_thr_lsb, reg_low_thr_msb;
+	uint16_t reg_high_thr_lsb, reg_high_thr_msb;
+	int rc = 0, btm_channel_num;
+
+	if (!adc_tm)
+		return -EINVAL;
+
+	tm_config.channel = adc_tm_sensor->sensor_num;
+	switch (trip) {
+	case ADC_TM_TRIP_HIGH_WARM:
+		tm_config.high_thr_temp = temp;
+		break;
+	case ADC_TM_TRIP_LOW_COOL:
+		tm_config.low_thr_temp = temp;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	rc = qpnp_adc_tm_scale_therm_voltage_pu2(&tm_config);
+	if (rc < 0) {
+		pr_err("Failed to lookup the adc-tm thresholds\n");
+		return rc;
+	}
+
+	trip_warm_thr0 = ((tm_config.low_thr_voltage << 24) >> 24);
+	trip_warm_thr1 = ((tm_config.low_thr_voltage << 16) >> 24);
+	trip_cool_thr0 = ((tm_config.high_thr_voltage << 24) >> 24);
+	trip_cool_thr1 = ((tm_config.high_thr_voltage << 16) >> 24);
+
+	btm_channel_num = adc_tm_sensor->btm_channel_num;
+	reg_low_thr_lsb = adc_tm_data[btm_channel_num].low_thr_lsb_addr;
+	reg_low_thr_msb = adc_tm_data[btm_channel_num].low_thr_msb_addr;
+	reg_high_thr_lsb = adc_tm_data[btm_channel_num].high_thr_lsb_addr;
+	reg_high_thr_msb = adc_tm_data[btm_channel_num].high_thr_msb_addr;
+
+	switch (trip) {
+	case ADC_TM_TRIP_HIGH_WARM:
+		rc = qpnp_adc_tm_write_reg(reg_low_thr_lsb, trip_cool_thr0);
+		if (rc) {
+			pr_err("adc-tm_tm read threshold err\n");
+			return rc;
+		}
+
+		rc = qpnp_adc_tm_write_reg(reg_low_thr_msb, trip_cool_thr1);
+		if (rc) {
+			pr_err("adc-tm_tm read threshold err\n");
+			return rc;
+		}
+	adc_tm_sensor->low_thr = tm_config.high_thr_voltage;
+	break;
+	case ADC_TM_TRIP_LOW_COOL:
+		rc = qpnp_adc_tm_write_reg(reg_high_thr_lsb, trip_warm_thr0);
+		if (rc) {
+			pr_err("adc-tm_tm read threshold err\n");
+			return rc;
+		}
+
+		rc = qpnp_adc_tm_write_reg(reg_high_thr_msb, trip_warm_thr1);
+		if (rc) {
+			pr_err("adc-tm_tm read threshold err\n");
+			return rc;
+		}
+	adc_tm_sensor->high_thr = tm_config.low_thr_voltage;
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void notify_uspace_qpnp_adc_tm_fn(struct work_struct *work)
+{
+	struct qpnp_adc_tm_sensor *adc_tm = container_of(work,
+		struct qpnp_adc_tm_sensor, work);
+
+	sysfs_notify(&adc_tm->tz_dev->device.kobj,
+					NULL, "btm");
+}
+
+static void notify_usb_fn(struct work_struct *work)
+{
+	struct qpnp_adc_tm_drv *adc_tm = container_of(work,
+		struct qpnp_adc_tm_drv, usbid_work);
+	int rc;
+	u8 status_low, status_high;
+
+	if (adc_tm->usb_id_param->threshold_notification != NULL) {
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_LOW,
+							&status_low);
+		if (rc) {
+			pr_err("adc-tm read low status failed\n");
+			return;
+		}
+
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_HIGH,
+							&status_high);
+		if (rc) {
+			pr_err("adc-tm read high status failed\n");
+			return;
+		}
+
+		if (status_low & 1)
+			adc_tm->usb_id_param->threshold_notification(
+			ADC_TM_LOW_STATE, adc_tm->usb_id_param->usbid_ctx);
+		else if (status_high & 1)
+			adc_tm->usb_id_param->threshold_notification(
+			ADC_TM_HIGH_STATE, adc_tm->usb_id_param->usbid_ctx);
+	}
+
+	return;
+}
+
+static void notify_batt_fn(struct work_struct *work)
+{
+	struct qpnp_adc_tm_drv *adc_tm = container_of(work,
+		struct qpnp_adc_tm_drv, batt_work);
+	int rc;
+	u8 status_low, status_high;
+
+	if (adc_tm->battery_param->threshold_notification != NULL) {
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_LOW,
+							&status_low);
+		if (rc) {
+			pr_err("adc-tm read low status failed\n");
+			return;
+		}
+
+		rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_HIGH,
+							&status_high);
+		if (rc) {
+			pr_err("adc-tm read high status failed\n");
+			return;
+		}
+
+		if (status_low & QPNP_ADC_TM_LOW_THR_INT_EN_M1)
+			adc_tm->battery_param->threshold_notification(
+							ADC_TM_LOW_STATE);
+		else if (status_high & QPNP_ADC_TM_HIGH_THR_INT_EN_M1)
+			adc_tm->battery_param->threshold_notification(
+							ADC_TM_HIGH_STATE);
+	}
+
+	return;
+}
+
+static int qpnp_adc_tm_activate_trip_type_fn(uint32_t btm_channel_num,
+	enum thermal_trip_activation_mode mode, u8 *data, uint32_t reg)
+{
+	u8 thr_int = 0;
+	int rc = 0;
+
+	rc = qpnp_adc_tm_read_reg(reg, &thr_int);
+	if (rc) {
+		pr_err("multi meas read failed\n");
+		return rc;
+	}
+
+	thr_int = adc_tm_data[btm_channel_num].low_thr_int_chan_en;
+
+	if (mode == THERMAL_TRIP_ACTIVATION_ENABLED)
+		thr_int |= *data;
+	else
+		thr_int &= ~*data;
+
+	rc = qpnp_adc_tm_write_reg(reg, thr_int);
+	if (rc)
+		pr_err("multi meas write failed\n");
+
+	return rc;
+}
+
+static int qpnp_adc_tm_activate_trip_type(struct thermal_zone_device *thermal,
+			int trip, enum thermal_trip_activation_mode mode)
+{
+	struct qpnp_adc_tm_sensor *adc_tm = thermal->devdata;
+	int rc = 0;
+	u8 thr_int_en = 0;
+
+	if (!adc_tm)
+		return -EINVAL;
+
+	switch (trip) {
+	case ADC_TM_TRIP_HIGH_WARM:
+		thr_int_en = adc_tm_data[adc_tm->btm_channel_num].
+							low_thr_int_chan_en;
+		rc = qpnp_adc_tm_activate_trip_type_fn(adc_tm->btm_channel_num,
+				mode, &thr_int_en, QPNP_ADC_TM_LOW_THR_INT_EN);
+		if (rc)
+			pr_err("channel:%x failed\n", adc_tm->btm_channel_num);
+	break;
+	case ADC_TM_TRIP_LOW_COOL:
+		thr_int_en = adc_tm_data[adc_tm->btm_channel_num].
+							high_thr_int_chan_en;
+		rc = qpnp_adc_tm_activate_trip_type_fn(adc_tm->btm_channel_num,
+				mode, &thr_int_en, QPNP_ADC_TM_HIGH_THR_INT_EN);
+		if (rc)
+			pr_err("channel:%x failed\n", adc_tm->btm_channel_num);
+	break;
+	default:
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int qpnp_adc_tm_read_status(void)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	u8 status_low = 0, status_high = 0, qpnp_adc_tm_meas_en = 0;
+	u8 adc_tm_low_enable = 0, adc_tm_high_enable = 0;
+	u8 thr_int_disable = 0;
+	int rc = 0, sensor_notify_num = 0;
+
+	rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_LOW, &status_low);
+	if (rc) {
+		pr_err("adc-tm-tm read status low failed with %d\n", rc);
+		goto fail;
+	}
+
+	rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_STATUS_HIGH, &status_high);
+	if (rc) {
+		pr_err("adc-tm-tm read status high failed with %d\n", rc);
+		goto fail;
+	}
+
+	/* Check which interrupt threshold is lower and measure against the
+	 * enabled channel */
+	rc = qpnp_adc_tm_read_reg(QPNP_ADC_TM_MULTI_MEAS_EN,
+							&qpnp_adc_tm_meas_en);
+	if (rc) {
+		pr_err("adc-tm-tm read status high failed with %d\n", rc);
+		goto fail;
+	}
+
+	adc_tm_low_enable = qpnp_adc_tm_meas_en & status_low;
+	adc_tm_high_enable = qpnp_adc_tm_meas_en & status_high;
+
+	if (adc_tm_high_enable) {
+		sensor_notify_num = (adc_tm_high_enable >> 3);
+		switch (adc_tm_high_enable) {
+		case 1:
+		case 2:
+		{
+			if (adc_tm_high_enable == 1)
+				thr_int_disable =
+					QPNP_ADC_TM_HIGH_THR_INT_EN_M0;
+			else if (adc_tm_high_enable == 2)
+				thr_int_disable =
+					QPNP_ADC_TM_HIGH_THR_INT_EN_M1;
+
+			rc = qpnp_adc_tm_meas_int_update(
+				QPNP_ADC_TM_HIGH_THR_INT_EN,
+				thr_int_disable, false);
+			if (rc < 0) {
+				pr_err("high threshold int read failed\n");
+				goto fail;
+			}
+
+			if (adc_tm_high_enable == 1)
+				schedule_work(&adc_tm->usbid_work);
+			else if (adc_tm_high_enable == 2)
+				schedule_work(&adc_tm->batt_work);
+		}
+		break;
+		case 4:
+		case 8:
+		case 16:
+		{
+			/* High voltage threshold is triggered by low temp */
+			rc = qpnp_adc_tm_activate_trip_type(
+				adc_tm->sensor[sensor_notify_num].tz_dev,
+				ADC_TM_TRIP_LOW_COOL,
+				THERMAL_TRIP_ACTIVATION_DISABLED);
+			if (rc < 0) {
+				pr_err("notify error:%d\n", sensor_notify_num);
+				goto fail;
+			}
+			schedule_work(&adc_tm->sensor[sensor_notify_num].work);
+		}
+		break;
+		default:
+			rc = -EINVAL;
+		}
+	}
+
+	if (adc_tm_low_enable) {
+		sensor_notify_num = (adc_tm_low_enable >> 3);
+		switch (adc_tm_low_enable) {
+		case 1:
+		case 2:
+		{
+			if (adc_tm_low_enable == 1)
+				thr_int_disable = QPNP_ADC_TM_LOW_THR_INT_EN_M0;
+			else if (adc_tm_low_enable == 2)
+				thr_int_disable = QPNP_ADC_TM_LOW_THR_INT_EN_M1;
+
+			rc = qpnp_adc_tm_meas_int_update(
+				QPNP_ADC_TM_LOW_THR_INT_EN,
+				thr_int_disable, false);
+			if (rc < 0) {
+				pr_err("low threshold int disable failed\n");
+				goto fail;
+			}
+
+			if (adc_tm_low_enable == 1)
+				schedule_work(&adc_tm->usbid_work);
+			else if (adc_tm_low_enable == 2)
+				schedule_work(&adc_tm->batt_work);
+		}
+		break;
+		case 4:
+		case 8:
+		case 16:
+		{
+			/* Low voltage threshold is triggered by high temp */
+			rc = qpnp_adc_tm_activate_trip_type(
+				adc_tm->sensor[sensor_notify_num].tz_dev,
+				ADC_TM_TRIP_HIGH_WARM,
+				THERMAL_TRIP_ACTIVATION_DISABLED);
+			if (rc < 0) {
+				pr_err("notify error:%d\n", sensor_notify_num);
+				goto fail;
+			}
+			schedule_work(&adc_tm->sensor[sensor_notify_num].work);
+		}
+		break;
+		default:
+			rc = -EINVAL;
+		}
+	}
+
+fail:
+	return rc;
+}
+
+static void qpnp_adc_tm_high_thr_work(struct work_struct *work)
+{
+	int rc;
+
+	rc = qpnp_adc_tm_read_status();
+
+	return;
+}
+DECLARE_WORK(trigger_completion_adc_tm_high_thr_work,
+					qpnp_adc_tm_high_thr_work);
+
+static irqreturn_t qpnp_adc_tm_high_thr_isr(int irq, void *data)
+{
+	schedule_work(&trigger_completion_adc_tm_high_thr_work);
+
+	return IRQ_HANDLED;
+}
+
+static void qpnp_adc_tm_low_thr_work(struct work_struct *work)
+{
+	int rc;
+
+	rc = qpnp_adc_tm_read_status();
+
+	return;
+}
+DECLARE_WORK(trigger_completion_adc_tm_low_thr_work, qpnp_adc_tm_low_thr_work);
+
+static irqreturn_t qpnp_adc_tm_low_thr_isr(int irq, void *data)
+{
+	schedule_work(&trigger_completion_adc_tm_low_thr_work);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t qpnp_adc_tm_isr(int irq, void *dev_id)
+{
+	struct qpnp_adc_tm_drv *adc_tm = dev_id;
+
+	complete(&adc_tm->adc->adc_rslt_completion);
+
+	return IRQ_HANDLED;
+}
+
+static int qpnp_adc_read_temp(struct thermal_zone_device *thermal,
+			     unsigned long *temp)
+{
+	struct qpnp_adc_tm_sensor *adc_tm_sensor = thermal->devdata;
+	struct qpnp_vadc_result result;
+	int rc = 0;
+
+	rc = qpnp_vadc_read(adc_tm_sensor->sensor_num, &result);
+	if (rc)
+		return rc;
+
+	*temp = result.physical;
+
+	return rc;
+}
+
+static struct thermal_zone_device_ops qpnp_adc_tm_thermal_ops = {
+	.get_temp = qpnp_adc_read_temp,
+	.get_mode = qpnp_adc_tm_get_mode,
+	.set_mode = qpnp_adc_tm_set_mode,
+	.get_trip_type = qpnp_adc_tm_get_trip_type,
+	.activate_trip_type = qpnp_adc_tm_activate_trip_type,
+	.get_trip_temp = qpnp_adc_tm_get_trip_temp,
+	.set_trip_temp = qpnp_adc_tm_set_trip_temp,
+};
+
+int32_t qpnp_adc_tm_usbid_configure(struct qpnp_adc_tm_usbid_param *param)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	uint32_t channel;
+	int rc = 0;
+
+	if (!adc_tm || !adc_tm->adc_tm_initialized)
+		return -ENODEV;
+
+	if (param->threshold_notification == NULL) {
+		pr_err("No USB_ID high/low voltage notificaton??\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&adc_tm->adc->adc_lock);
+
+	adc_tm->adc->amux_prop->amux_channel = LR_MUX10_PU2_AMUX_USB_ID_LV;
+	channel = LR_MUX10_PU2_AMUX_USB_ID_LV;
+	adc_tm->adc->amux_prop->decimation =
+			adc_tm->adc->adc_channels[channel].adc_decimation;
+	adc_tm->adc->amux_prop->hw_settle_time =
+			adc_tm->adc->adc_channels[channel].hw_settle_time;
+	adc_tm->adc->amux_prop->fast_avg_setup =
+			adc_tm->adc->adc_channels[channel].fast_avg_setup;
+	adc_tm->adc->amux_prop->mode_sel =
+		ADC_OP_MEASUREMENT_INTERVAL << QPNP_OP_MODE_SHIFT;
+	adc_tm->adc->amux_prop->chan_prop->meas_interval1 =
+						ADC_MEAS1_INTERVAL_1S;
+	qpnp_adc_usb_scaler(param, &adc_tm->adc->amux_prop->chan_prop->low_thr,
+			&adc_tm->adc->amux_prop->chan_prop->high_thr);
+	adc_tm->adc->amux_prop->chan_prop->tm_channel_select =
+					QPNP_ADC_TM_M0_ADC_CH_SEL_CTL;
+	adc_tm->adc->amux_prop->chan_prop->timer_select =
+					ADC_MEAS_TIMER_SELECT1;
+	adc_tm->adc->amux_prop->chan_prop->state_request =
+					param->state_request;
+	rc = qpnp_adc_tm_configure(adc_tm->adc->amux_prop);
+	if (rc) {
+		pr_err("adc-tm configure failed with %d\n", rc);
+		goto fail_unlock;
+	}
+
+	adc_tm->usb_id_param = param;
+
+fail_unlock:
+	mutex_unlock(&adc_tm->adc->adc_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(qpnp_adc_tm_usbid_configure);
+
+static int32_t qpnp_adc_tm_chan_usbid_chan_btm_end(
+				uint32_t btm_chan_num)
+{
+	int32_t rc = 0;
+
+	rc = qpnp_adc_tm_meas_int_update(QPNP_ADC_TM_LOW_THR_INT_EN,
+		adc_tm_data[btm_chan_num].low_thr_int_chan_en,
+		false);
+	if (rc < 0) {
+		pr_err("low threshold int write failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_meas_int_update(QPNP_ADC_TM_HIGH_THR_INT_EN,
+		adc_tm_data[btm_chan_num].high_thr_int_chan_en,
+		false);
+	if (rc < 0) {
+		pr_err("high threshold int enable failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_meas_int_update(QPNP_ADC_TM_MULTI_MEAS_EN,
+		adc_tm_data[btm_chan_num].multi_meas_en,
+		false);
+	if (rc < 0) {
+		pr_err("multi measurement en failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_enable(false);
+	if (rc < 0)
+		pr_err("TM disable failed\n");
+
+	return rc;
+}
+
+int32_t qpnp_adc_tm_usbid_end(void)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	int rc = 0;
+
+	if (!adc_tm || !adc_tm->adc_tm_initialized)
+		return -ENODEV;
+
+	mutex_lock(&adc_tm->adc->adc_lock);
+
+	rc = qpnp_adc_tm_chan_usbid_chan_btm_end(
+				QPNP_ADC_TM_M0_ADC_CH_SEL_CTL);
+	if (rc < 0)
+		pr_err("disabling thresholds for usb channel failed\n");
+
+	mutex_unlock(&adc_tm->adc->adc_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(qpnp_adc_tm_usbid_end);
+
+int32_t qpnp_adc_tm_btm_configure(struct qpnp_adc_tm_btm_param *param)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	uint32_t channel;
+	int rc = 0;
+
+	if (!adc_tm || !adc_tm->adc_tm_initialized)
+		return -ENODEV;
+
+	if (param->threshold_notification == NULL) {
+		pr_err("No battery high/low temp notificaton??\n");
+		return -EINVAL;
+	}
+
+	mutex_lock(&adc_tm->adc->adc_lock);
+
+	adc_tm->adc->amux_prop->amux_channel = LR_MUX1_BATT_THERM;
+	channel = LR_MUX1_BATT_THERM;
+	adc_tm->adc->amux_prop->decimation =
+			adc_tm->adc->adc_channels[channel].adc_decimation;
+	adc_tm->adc->amux_prop->hw_settle_time =
+			adc_tm->adc->adc_channels[channel].hw_settle_time;
+	adc_tm->adc->amux_prop->fast_avg_setup =
+			adc_tm->adc->adc_channels[channel].fast_avg_setup;
+	adc_tm->adc->amux_prop->mode_sel =
+		ADC_OP_MEASUREMENT_INTERVAL << QPNP_OP_MODE_SHIFT;
+	adc_tm->adc->amux_prop->chan_prop->meas_interval2 =
+						ADC_MEAS2_INTERVAL_1S;
+	qpnp_adc_btm_scaler(param, &adc_tm->adc->amux_prop->chan_prop->low_thr,
+			&adc_tm->adc->amux_prop->chan_prop->high_thr);
+	adc_tm->adc->amux_prop->chan_prop->tm_channel_select =
+					QPNP_ADC_TM_M1_ADC_CH_SEL_CTL;
+	adc_tm->adc->amux_prop->chan_prop->timer_select =
+					ADC_MEAS_TIMER_SELECT2;
+	adc_tm->adc->amux_prop->chan_prop->state_request =
+					param->state_request;
+	rc = qpnp_adc_tm_configure(adc_tm->adc->amux_prop);
+	if (rc) {
+		pr_err("adc-tm configure failed with %d\n", rc);
+		goto fail_unlock;
+	}
+
+	adc_tm->battery_param = param;
+
+fail_unlock:
+	mutex_unlock(&adc_tm->adc->adc_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(qpnp_adc_tm_btm_configure);
+
+int32_t qpnp_adc_tm_btm_end(void)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+	int rc = 0;
+
+	if (!adc_tm || !adc_tm->adc_tm_initialized)
+		return -ENODEV;
+
+	mutex_lock(&adc_tm->adc->adc_lock);
+
+	rc = qpnp_adc_tm_chan_usbid_chan_btm_end(
+				QPNP_ADC_TM_M1_ADC_CH_SEL_CTL);
+	if (rc < 0)
+		pr_err("disabling thresholds for batt channel failed\n");
+
+	mutex_unlock(&adc_tm->adc->adc_lock);
+
+	return rc;
+}
+EXPORT_SYMBOL(qpnp_adc_tm_btm_end);
+
+int32_t qpnp_adc_tm_is_ready(void)
+{
+	struct qpnp_adc_tm_drv *adc_tm = qpnp_adc_tm;
+
+	if (!adc_tm || !adc_tm->adc_tm_initialized)
+		return -EPROBE_DEFER;
+	else
+		return 0;
+}
+EXPORT_SYMBOL(qpnp_adc_tm_is_ready);
+
+static int __devinit qpnp_adc_tm_probe(struct spmi_device *spmi)
+{
+	struct device_node *node = spmi->dev.of_node, *child;
+	struct qpnp_adc_tm_drv *adc_tm;
+	struct qpnp_adc_drv *adc_qpnp;
+	int32_t count_adc_channel_list = 0, rc, i = 0, j = 0;
+	u8 thr_init = 0;
+
+	if (!node)
+		return -EINVAL;
+
+	if (qpnp_adc_tm) {
+		pr_err("adc-tm already in use\n");
+		return -EBUSY;
+	}
+
+	for_each_child_of_node(node, child)
+		count_adc_channel_list++;
+
+	if (!count_adc_channel_list) {
+		pr_err("No channel listing\n");
+		return -EINVAL;
+	}
+
+	adc_tm = devm_kzalloc(&spmi->dev, sizeof(struct qpnp_adc_tm_drv) +
+			(count_adc_channel_list *
+			sizeof(struct qpnp_adc_tm_sensor)),
+				GFP_KERNEL);
+	if (!adc_tm) {
+		dev_err(&spmi->dev, "Unable to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	adc_qpnp = devm_kzalloc(&spmi->dev, sizeof(struct qpnp_adc_drv),
+			GFP_KERNEL);
+	if (!adc_qpnp) {
+		dev_err(&spmi->dev, "Unable to allocate memory\n");
+		return -ENOMEM;
+	}
+
+	adc_tm->adc = adc_qpnp;
+
+	rc = qpnp_adc_get_devicetree_data(spmi, adc_tm->adc);
+	if (rc) {
+		dev_err(&spmi->dev, "failed to read device tree\n");
+		return rc;
+	}
+
+	/* Register the ADC peripheral interrupt */
+	adc_tm->adc->adc_high_thr_irq = spmi_get_irq_byname(spmi,
+						NULL, "high-thr-en-set");
+	if (adc_tm->adc->adc_high_thr_irq < 0) {
+		pr_err("Invalid irq\n");
+		return -ENXIO;
+	}
+
+	adc_tm->adc->adc_low_thr_irq = spmi_get_irq_byname(spmi,
+						NULL, "low-thr-en-set");
+	if (adc_tm->adc->adc_low_thr_irq < 0) {
+		pr_err("Invalid irq\n");
+		return -ENXIO;
+	}
+
+	rc = devm_request_irq(&spmi->dev, adc_tm->adc->adc_irq_eoc,
+				qpnp_adc_tm_isr, IRQF_TRIGGER_RISING,
+				"qpnp_adc_tm_interrupt", adc_tm);
+	if (rc) {
+		dev_err(&spmi->dev,
+			"failed to request adc irq with error %d\n", rc);
+		return rc;
+	} else {
+		enable_irq_wake(adc_tm->adc->adc_irq_eoc);
+	}
+
+	rc = devm_request_irq(&spmi->dev, adc_tm->adc->adc_high_thr_irq,
+				qpnp_adc_tm_high_thr_isr,
+		IRQF_TRIGGER_RISING, "qpnp_adc_tm_high_interrupt", adc_tm);
+	if (rc) {
+		dev_err(&spmi->dev, "failed to request adc irq\n");
+		return rc;
+	} else {
+		enable_irq_wake(adc_tm->adc->adc_high_thr_irq);
+	}
+
+	rc = devm_request_irq(&spmi->dev, adc_tm->adc->adc_low_thr_irq,
+				qpnp_adc_tm_low_thr_isr,
+		IRQF_TRIGGER_RISING, "qpnp_adc_tm_low_interrupt", adc_tm);
+	if (rc) {
+		dev_err(&spmi->dev, "failed to request adc irq\n");
+		return rc;
+	} else {
+		enable_irq_wake(adc_tm->adc->adc_low_thr_irq);
+	}
+
+	for_each_child_of_node(node, child) {
+		char name[25];
+		int btm_channel_num;
+		rc = of_property_read_u32(child,
+				"qcom,btm-channel-number", &btm_channel_num);
+		if (rc) {
+			pr_err("Invalid btm channel number\n");
+			return -EINVAL;
+		}
+
+		if ((btm_channel_num != QPNP_ADC_TM_M0_ADC_CH_SEL_CTL) &&
+			(btm_channel_num != QPNP_ADC_TM_M1_ADC_CH_SEL_CTL)) {
+			/* Register with the thermal zone */
+			adc_tm->sensor[i].mode = THERMAL_DEVICE_DISABLED;
+			snprintf(name, sizeof(name), "qpnp_adc_tm_sensor%d", i);
+			adc_tm->sensor[i].sensor_num =
+				adc_tm->adc->adc_channels[j].channel_num;
+			adc_tm->sensor[i].btm_channel_num = btm_channel_num;
+			adc_tm->sensor[i].meas_interval =
+				QPNP_ADC_TM_MEAS_INTERVAL;
+			adc_tm->sensor[i].low_thr = QPNP_ADC_TM_M0_LOW_THR;
+			adc_tm->sensor[i].high_thr = QPNP_ADC_TM_M0_HIGH_THR;
+			adc_tm->sensor[i].tz_dev =
+				thermal_zone_device_register(name,
+				ADC_TM_TRIP_NUM,
+				&adc_tm->sensor[i],
+				&qpnp_adc_tm_thermal_ops, 0, 0, 0, 0);
+			if (IS_ERR(adc_tm->sensor[i].tz_dev))
+				pr_err("thermal device register failed.\n");
+				INIT_WORK(&adc_tm->sensor[i].work,
+					notify_uspace_qpnp_adc_tm_fn);
+			i++;
+		}
+		j++;
+	}
+	INIT_WORK(&adc_tm->usbid_work, notify_usb_fn);
+	INIT_WORK(&adc_tm->batt_work, notify_batt_fn);
+	qpnp_adc_tm = adc_tm;
+	dev_set_drvdata(&spmi->dev, adc_tm);
+	rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_HIGH_THR_INT_EN, thr_init);
+	if (rc < 0) {
+		pr_err("high thr init failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_LOW_THR_INT_EN, thr_init);
+	if (rc < 0) {
+		pr_err("low thr init failed\n");
+		return rc;
+	}
+
+	rc = qpnp_adc_tm_write_reg(QPNP_ADC_TM_MULTI_MEAS_EN, thr_init);
+	if (rc < 0) {
+		pr_err("multi meas en failed\n");
+		return rc;
+	}
+
+	adc_tm->adc_tm_initialized = true;
+
+	return 0;
+}
+
+static int __devexit qpnp_adc_tm_remove(struct spmi_device *spmi)
+{
+	struct qpnp_adc_tm_drv *adc_tm = dev_get_drvdata(&spmi->dev);
+	struct device_node *node = spmi->dev.of_node;
+	struct device_node *child;
+	int i = 0;
+
+	for_each_child_of_node(node, child) {
+		thermal_zone_device_unregister(adc_tm->sensor[i].tz_dev);
+		i++;
+	}
+
+	adc_tm->adc_tm_initialized = false;
+	dev_set_drvdata(&spmi->dev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id qpnp_adc_tm_match_table[] = {
+	{	.compatible = "qcom,qpnp-adc-tm" },
+	{}
+};
+
+static struct spmi_driver qpnp_adc_tm_driver = {
+	.driver		= {
+		.name	= "qcom,qpnp-adc-tm",
+		.of_match_table = qpnp_adc_tm_match_table,
+	},
+	.probe		= qpnp_adc_tm_probe,
+	.remove		= qpnp_adc_tm_remove,
+};
+
+static int __init qpnp_adc_tm_init(void)
+{
+	return spmi_driver_register(&qpnp_adc_tm_driver);
+}
+module_init(qpnp_adc_tm_init);
+
+static void __exit qpnp_adc_tm_exit(void)
+{
+	spmi_driver_unregister(&qpnp_adc_tm_driver);
+}
+module_exit(qpnp_adc_tm_exit);
+
+MODULE_DESCRIPTION("QPNP PMIC ADC Threshold Monitoring driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/msm_serial_hs_lite.c b/drivers/tty/serial/msm_serial_hs_lite.c
index 3de990c..72a12d1 100644
--- a/drivers/tty/serial/msm_serial_hs_lite.c
+++ b/drivers/tty/serial/msm_serial_hs_lite.c
@@ -669,15 +669,15 @@
 	 * Check requested baud rate and for higher baud rate than 460800,
 	 * calculate required uart clock frequency and set the same.
 	 */
-	if (baud > 460800) {
-
+	if (baud > 460800)
 		port->uartclk = baud * 16;
-		if (clk_set_rate(msm_hsl_port->clk, port->uartclk)) {
-			pr_err("%s(): Error setting uartclk rate %u\n",
-						__func__, port->uartclk);
-			WARN_ON(1);
-			return;
-		}
+	else
+		port->uartclk = 7372800;
+
+	if (clk_set_rate(msm_hsl_port->clk, port->uartclk)) {
+		pr_err("Error: setting uartclk rate %u\n", port->uartclk);
+		WARN_ON(1);
+		return;
 	}
 
 	/* Set timeout to be ~600x the character transmit time */
@@ -692,9 +692,9 @@
 	 * Configure Rx Watermark as 3/4 size of Rx FIFO.
 	 * RFWR register takes value in Words for UARTDM Core
 	 * whereas it is consider to be in Bytes for UART Core.
-	 * Hence configuring Rx Watermark as 12 Words.
+	 * Hence configuring Rx Watermark as 48 Words.
 	 */
-	watermark = (port->fifosize * 3) / (4*4);
+	watermark = (port->fifosize * 3) / 4;
 	msm_hsl_write(port, watermark, regmap[vid][UARTDM_RFWR]);
 
 	/* set TX watermark */
@@ -775,18 +775,15 @@
 #endif
 	pm_runtime_get_sync(port->dev);
 
-	/* Set RFR Level as 3/4 of UARTDM FIFO Size */
+	/*
+	 * Set RFR Level as 3/4 of UARTDM FIFO Size
+	 * i.e. 48 Words = 192 bytes as Rx FIFO is 64 words ( 256 bytes).
+	 */
 	if (likely(port->fifosize > 48))
 		rfr_level = port->fifosize - 16;
 	else
 		rfr_level = port->fifosize;
 
-	/*
-	 * Use rfr_level value in Words to program
-	 * MR1 register for UARTDM Core.
-	 */
-	rfr_level = (rfr_level / 4);
-
 	spin_lock_irqsave(&port->lock, flags);
 
 	vid = msm_hsl_port->ver_id;
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index c0b4b57..265a685 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -300,19 +300,7 @@
 	}
 }
 
-/* XHCI reset, resets other CORE registers as well, re-init those */
-void dwc3_post_host_reset_core_init(struct dwc3 *dwc)
-{
-	/*
-	 * XHCI reset clears EVENT buffer register as well, re-init
-	 * EVENT buffers and also do device specific re-initialization
-	 */
-	dwc3_event_buffers_setup(dwc);
-
-	dwc3_gadget_restart(dwc);
-}
-
-static void __devinit dwc3_cache_hwparams(struct dwc3 *dwc)
+static void dwc3_cache_hwparams(struct dwc3 *dwc)
 {
 	struct dwc3_hwparams	*parms = &dwc->hwparams;
 
@@ -333,7 +321,7 @@
  *
  * Returns 0 on success otherwise negative errno.
  */
-static int __devinit dwc3_core_init(struct dwc3 *dwc)
+static int dwc3_core_init(struct dwc3 *dwc)
 {
 	unsigned long		timeout;
 	u32			reg;
@@ -434,11 +422,13 @@
 		dwc3_writel(dwc->regs, DWC3_GUSB3PIPECTL(0), reg);
 	}
 
-	ret = dwc3_alloc_event_buffers(dwc, DWC3_EVENT_BUFFERS_SIZE);
-	if (ret) {
-		dev_err(dwc->dev, "failed to allocate event buffers\n");
-		ret = -ENOMEM;
-		goto err1;
+	if (!dwc->ev_buffs) {
+		ret = dwc3_alloc_event_buffers(dwc, DWC3_EVENT_BUFFERS_SIZE);
+		if (ret) {
+			dev_err(dwc->dev, "failed to allocate event buffers\n");
+			ret = -ENOMEM;
+			goto err1;
+		}
 	}
 
 	ret = dwc3_event_buffers_setup(dwc);
@@ -462,6 +452,13 @@
 	dwc3_free_event_buffers(dwc);
 }
 
+/* XHCI reset, resets other CORE registers as well, re-init those */
+void dwc3_post_host_reset_core_init(struct dwc3 *dwc)
+{
+	dwc3_core_init(dwc);
+	dwc3_gadget_restart(dwc);
+}
+
 #define DWC3_ALIGN_MASK		(16 - 1)
 
 static int __devinit dwc3_probe(struct platform_device *pdev)
diff --git a/drivers/usb/dwc3/dwc3-msm.c b/drivers/usb/dwc3/dwc3-msm.c
index d416904..a3abe23 100644
--- a/drivers/usb/dwc3/dwc3-msm.c
+++ b/drivers/usb/dwc3/dwc3-msm.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -34,16 +34,31 @@
 #include <linux/usb/msm_hsusb.h>
 #include <linux/regulator/consumer.h>
 #include <linux/power_supply.h>
+#include <linux/qpnp/qpnp-adc.h>
 
 #include <mach/rpm-regulator.h>
 #include <mach/rpm-regulator-smd.h>
 #include <mach/msm_xo.h>
 #include <mach/msm_bus.h>
+#include <mach/clk.h>
 
 #include "dwc3_otg.h"
 #include "core.h"
 #include "gadget.h"
 
+/* ADC threshold values */
+static int adc_low_threshold = 700;
+module_param(adc_low_threshold, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(adc_low_threshold, "ADC ID Low voltage threshold");
+
+static int adc_high_threshold = 950;
+module_param(adc_high_threshold, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(adc_high_threshold, "ADC ID High voltage threshold");
+
+static int adc_meas_interval = ADC_MEAS1_INTERVAL_1S;
+module_param(adc_meas_interval, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(adc_meas_interval, "ADC ID polling period");
+
 /**
  *  USB DBM Hardware registers.
  *
@@ -160,6 +175,9 @@
 	struct usb_phy		*otg_xceiv;
 	struct delayed_work	chg_work;
 	enum usb_chg_state	chg_state;
+	struct qpnp_adc_tm_usbid_param	adc_param;
+	struct delayed_work	init_adc_work;
+	bool			id_adc_detect;
 	u8			dcd_retries;
 	u32			bus_perf_client;
 	struct msm_bus_scale_pdata	*bus_scale_table;
@@ -171,6 +189,7 @@
 	unsigned int		vdd_low_vol_level;
 	unsigned int		vdd_high_vol_level;
 	bool			vbus_active;
+	bool			ext_inuse;
 };
 
 #define USB_HSPHY_3P3_VOL_MIN		3050000 /* uV */
@@ -188,6 +207,8 @@
 static struct dwc3_msm *context;
 static u64 dwc3_msm_dma_mask = DMA_BIT_MASK(64);
 
+static struct usb_ext_notification *usb_ext;
+
 /**
  *
  * Read register with debug info.
@@ -894,6 +915,33 @@
 }
 EXPORT_SYMBOL(msm_ep_unconfig);
 
+/**
+ * msm_register_usb_ext_notification: register for event notification
+ * @info: pointer to client usb_ext_notification structure. May be NULL.
+ *
+ * @return int - 0 on success, negative on error
+ */
+int msm_register_usb_ext_notification(struct usb_ext_notification *info)
+{
+	pr_debug("%s usb_ext: %p\n", __func__, info);
+
+	if (info) {
+		if (usb_ext) {
+			pr_err("%s: already registered\n", __func__);
+			return -EEXIST;
+		}
+
+		if (!info->notify) {
+			pr_err("%s: notify is NULL\n", __func__);
+			return -EINVAL;
+		}
+	}
+
+	usb_ext = info;
+	return 0;
+}
+EXPORT_SYMBOL(msm_register_usb_ext_notification);
+
 /* HSPHY */
 static int dwc3_hsusb_config_vddcx(int high)
 {
@@ -1099,6 +1147,95 @@
 	return rc < 0 ? rc : 0;
 }
 
+static int dwc3_msm_link_clk_reset(bool assert)
+{
+	int ret = 0;
+	struct dwc3_msm *mdwc = context;
+
+	if (assert) {
+		/* Using asynchronous block reset to the hardware */
+		dev_dbg(mdwc->dev, "block_reset ASSERT\n");
+		clk_disable_unprepare(mdwc->ref_clk);
+		clk_disable_unprepare(mdwc->iface_clk);
+		clk_disable_unprepare(mdwc->core_clk);
+		ret = clk_reset(mdwc->core_clk, CLK_RESET_ASSERT);
+		if (ret)
+			dev_err(mdwc->dev, "dwc3 core_clk assert failed\n");
+	} else {
+		dev_dbg(mdwc->dev, "block_reset DEASSERT\n");
+		ret = clk_reset(mdwc->core_clk, CLK_RESET_DEASSERT);
+		ndelay(200);
+		clk_prepare_enable(mdwc->core_clk);
+		clk_prepare_enable(mdwc->ref_clk);
+		clk_prepare_enable(mdwc->iface_clk);
+		if (ret)
+			dev_err(mdwc->dev, "dwc3 core_clk deassert failed\n");
+	}
+
+	return ret;
+}
+
+/* Initialize QSCRATCH registers for HSPHY and SSPHY operation */
+static void dwc3_msm_qscratch_reg_init(struct dwc3_msm *msm)
+{
+	u32 data = 0;
+
+	/* SSPHY Initialization: Use ref_clk from pads and set its parameters */
+	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210002);
+	msleep(30);
+	/* Assert SSPHY reset */
+	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210082);
+	usleep_range(2000, 2200);
+	/* De-assert SSPHY reset - power and ref_clock must be ON */
+	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210002);
+	usleep_range(2000, 2200);
+	/* Ref clock must be stable now, enable ref clock for HS mode */
+	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210102);
+	usleep_range(2000, 2200);
+	/*
+	 * HSPHY Initialization: Enable UTMI clock and clamp enable HVINTs,
+	 * and disable RETENTION (power-on default is ENABLED)
+	 */
+	dwc3_msm_write_reg(msm->base, HS_PHY_CTRL_REG, 0x5220bb2);
+	usleep_range(2000, 2200);
+	/* Disable (bypass) VBUS and ID filters */
+	dwc3_msm_write_reg(msm->base, QSCRATCH_GENERAL_CFG, 0x78);
+
+	/*
+	 * WORKAROUND: There is SSPHY suspend bug due to which USB enumerates
+	 * in HS mode instead of SS mode. Workaround it by asserting
+	 * LANE0.TX_ALT_BLOCK.EN_ALT_BUS to enable TX to use alt bus mode
+	 */
+	data = dwc3_msm_ssusb_read_phycreg(msm->base, 0x102D);
+	data |= (1 << 7);
+	dwc3_msm_ssusb_write_phycreg(msm->base, 0x102D, data);
+
+	data = dwc3_msm_ssusb_read_phycreg(msm->base, 0x1010);
+	data &= ~0xFF0;
+	data |= 0x40;
+	dwc3_msm_ssusb_write_phycreg(msm->base, 0x1010, data);
+}
+
+static void dwc3_msm_block_reset(void)
+{
+	struct dwc3_msm *mdwc = context;
+	int ret  = 0;
+
+	ret = dwc3_msm_link_clk_reset(1);
+	if (ret)
+		return;
+
+	usleep_range(1000, 1200);
+	ret = dwc3_msm_link_clk_reset(0);
+	if (ret)
+		return;
+
+	usleep_range(10000, 12000);
+
+	/* Reinitialize QSCRATCH registers after block reset */
+	dwc3_msm_qscratch_reg_init(mdwc);
+}
+
 static void dwc3_chg_enable_secondary_det(struct dwc3_msm *mdwc)
 {
 	u32 chg_ctrl;
@@ -1248,6 +1385,10 @@
 		/* fall through */
 	case USB_CHG_STATE_DETECTED:
 		dwc3_chg_block_reset(mdwc);
+		/* Enable VDP_SRC */
+		if (mdwc->charger.chg_type == DWC3_DCP_CHARGER)
+			dwc3_msm_write_readback(mdwc->base,
+					CHARGING_DET_CTRL_REG, 0x1F, 0x10);
 		dev_dbg(mdwc->dev, "chg_type = %s\n",
 			chg_to_string(mdwc->charger.chg_type));
 		mdwc->charger.notify_detection_complete(mdwc->otg_xceiv->otg,
@@ -1279,6 +1420,7 @@
 static int dwc3_msm_suspend(struct dwc3_msm *mdwc)
 {
 	int ret;
+	bool dcp;
 
 	dev_dbg(mdwc->dev, "%s: entering lpm\n", __func__);
 
@@ -1287,23 +1429,29 @@
 		return 0;
 	}
 
+	if (mdwc->hs_phy_irq)
+		disable_irq(mdwc->hs_phy_irq);
+
 	if (cancel_delayed_work_sync(&mdwc->chg_work))
 		dev_dbg(mdwc->dev, "%s: chg_work was pending\n", __func__);
 	if (mdwc->chg_state != USB_CHG_STATE_DETECTED) {
 		/* charger detection wasn't complete; re-init flags */
 		mdwc->chg_state = USB_CHG_STATE_UNDEFINED;
 		mdwc->charger.chg_type = DWC3_INVALID_CHARGER;
+		dwc3_msm_write_readback(mdwc->base, CHARGING_DET_CTRL_REG,
+								0x37, 0x0);
 	}
 
+	dcp = mdwc->charger.chg_type == DWC3_DCP_CHARGER;
+
 	/* Sequence to put hardware in low power state:
 	 * 1. Set OTGDISABLE to disable OTG block in HSPHY (saves power)
-	 * 2. Clear charger detection control fields
+	 * 2. Clear charger detection control fields (performed above)
 	 * 3. SUSPEND PHY and turn OFF core clock after some delay
 	 * 4. Clear interrupt latch register and enable BSV, ID HV interrupts
 	 * 5. Enable PHY retention
 	 */
 	dwc3_msm_write_readback(mdwc->base, HS_PHY_CTRL_REG, 0x1000, 0x1000);
-	dwc3_msm_write_readback(mdwc->base, CHARGING_DET_CTRL_REG, 0x37, 0x0);
 	dwc3_msm_write_readback(mdwc->base, HS_PHY_CTRL_REG,
 						0xC00000, 0x800000);
 
@@ -1323,7 +1471,8 @@
 
 	dwc3_msm_write_reg(mdwc->base, HS_PHY_IRQ_STAT_REG, 0xFFF);
 	dwc3_msm_write_readback(mdwc->base, HS_PHY_CTRL_REG, 0x18000, 0x18000);
-	dwc3_msm_write_readback(mdwc->base, HS_PHY_CTRL_REG, 0x2, 0x0);
+	if (!dcp)
+		dwc3_msm_write_readback(mdwc->base, HS_PHY_CTRL_REG, 0x2, 0x0);
 
 	/* make sure above writes are completed before turning off clocks */
 	wmb();
@@ -1343,7 +1492,7 @@
 			dev_err(mdwc->dev, "Failed to reset bus bw vote\n");
 	}
 
-	if (mdwc->otg_xceiv && mdwc->ext_xceiv.otg_capability)
+	if (mdwc->otg_xceiv && mdwc->ext_xceiv.otg_capability && !dcp)
 		dwc3_hsusb_ldo_enable(0);
 
 	dwc3_ssusb_ldo_enable(0);
@@ -1354,12 +1503,16 @@
 
 	dev_info(mdwc->dev, "DWC3 in low power mode\n");
 
+	if (mdwc->hs_phy_irq)
+		enable_irq(mdwc->hs_phy_irq);
+
 	return 0;
 }
 
 static int dwc3_msm_resume(struct dwc3_msm *mdwc)
 {
 	int ret;
+	bool dcp;
 
 	dev_dbg(mdwc->dev, "%s: exiting lpm\n", __func__);
 
@@ -1383,7 +1536,8 @@
 		dev_err(mdwc->dev, "%s failed to vote for TCXO buffer%d\n",
 						__func__, ret);
 
-	if (mdwc->otg_xceiv && mdwc->ext_xceiv.otg_capability)
+	dcp = mdwc->charger.chg_type == DWC3_DCP_CHARGER;
+	if (mdwc->otg_xceiv && mdwc->ext_xceiv.otg_capability && !dcp)
 		dwc3_hsusb_ldo_enable(1);
 
 	dwc3_ssusb_ldo_enable(1);
@@ -1465,7 +1619,7 @@
 	}
 }
 
-static u32 debug_id, debug_bsv, debug_connect;
+static u32 debug_id = true, debug_bsv, debug_connect;
 
 static int dwc3_connect_show(struct seq_file *s, void *unused)
 {
@@ -1611,7 +1765,6 @@
 		if (mdwc->otg_xceiv && (mdwc->ext_xceiv.otg_capability ||
 							!init)) {
 			mdwc->ext_xceiv.bsv = val->intval;
-			mdwc->ext_xceiv.id = DWC3_ID_FLOAT;
 			if (atomic_read(&mdwc->in_lpm)) {
 				dev_dbg(mdwc->dev,
 					"%s received in LPM\n", __func__);
@@ -1652,6 +1805,126 @@
 	POWER_SUPPLY_PROP_SCOPE,
 };
 
+static void dwc3_init_adc_work(struct work_struct *w);
+
+static void dwc3_ext_notify_online(int on)
+{
+	struct dwc3_msm *mdwc = context;
+
+	if (!mdwc) {
+		pr_err("%s: DWC3 driver already removed\n", __func__);
+		return;
+	}
+
+	dev_dbg(mdwc->dev, "notify %s%s\n", on ? "" : "dis", "connected");
+
+	if (!on) {
+		/* external client offline; revert back to USB */
+		mdwc->ext_inuse = false;
+		queue_delayed_work(system_nrt_wq, &mdwc->resume_work, 0);
+	}
+}
+
+static bool dwc3_ext_trigger_handled(struct dwc3_msm *mdwc,
+				     enum dwc3_id_state id)
+{
+	int ret;
+
+	if (!usb_ext)
+		return false;
+
+	ret = usb_ext->notify(usb_ext->ctxt, id, dwc3_ext_notify_online);
+	dev_dbg(mdwc->dev, "%s: external event handler returned %d\n", __func__,
+			ret);
+	mdwc->ext_inuse = ret == 0;
+	return mdwc->ext_inuse;
+}
+
+static void dwc3_adc_notification(enum qpnp_tm_state state, void *ctx)
+{
+	struct dwc3_msm *mdwc = ctx;
+
+	if (state >= ADC_TM_STATE_NUM) {
+		pr_err("%s: invalid notification %d\n", __func__, state);
+		return;
+	}
+
+	dev_dbg(mdwc->dev, "%s: state = %s\n", __func__,
+			state == ADC_TM_HIGH_STATE ? "high" : "low");
+
+	if (state == ADC_TM_HIGH_STATE) {
+		mdwc->ext_xceiv.id = DWC3_ID_FLOAT;
+		mdwc->adc_param.state_request = ADC_TM_LOW_THR_ENABLE;
+	} else {
+		mdwc->ext_xceiv.id = DWC3_ID_GROUND;
+		mdwc->adc_param.state_request = ADC_TM_HIGH_THR_ENABLE;
+	}
+
+	/* Give external client a chance to handle, otherwise notify OTG */
+	if (!mdwc->ext_inuse &&
+			!dwc3_ext_trigger_handled(mdwc, mdwc->ext_xceiv.id))
+		queue_delayed_work(system_nrt_wq, &mdwc->resume_work, 0);
+
+	/* re-arm ADC interrupt */
+	qpnp_adc_tm_usbid_configure(&mdwc->adc_param);
+}
+
+static void dwc3_init_adc_work(struct work_struct *w)
+{
+	struct dwc3_msm *mdwc = container_of(w, struct dwc3_msm,
+							init_adc_work.work);
+	int ret;
+
+	ret = qpnp_adc_tm_is_ready();
+	if (ret == -EPROBE_DEFER) {
+		queue_delayed_work(system_nrt_wq, to_delayed_work(w),
+					msecs_to_jiffies(100));
+		return;
+	}
+
+	mdwc->adc_param.low_thr = adc_low_threshold;
+	mdwc->adc_param.high_thr = adc_high_threshold;
+	mdwc->adc_param.timer_interval = adc_meas_interval;
+	mdwc->adc_param.state_request = ADC_TM_HIGH_LOW_THR_ENABLE;
+	mdwc->adc_param.usbid_ctx = mdwc;
+	mdwc->adc_param.threshold_notification = dwc3_adc_notification;
+
+	ret = qpnp_adc_tm_usbid_configure(&mdwc->adc_param);
+	if (ret) {
+		dev_err(mdwc->dev, "%s: request ADC error %d\n", __func__, ret);
+		return;
+	}
+
+	mdwc->id_adc_detect = true;
+}
+
+static ssize_t adc_enable_show(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%s\n", context->id_adc_detect ?
+						"enabled" : "disabled");
+}
+
+static ssize_t adc_enable_store(struct device *dev,
+		struct device_attribute *attr, const char
+		*buf, size_t size)
+{
+	if (!strnicmp(buf, "enable", 6)) {
+		if (!context->id_adc_detect)
+			dwc3_init_adc_work(&context->init_adc_work.work);
+		return size;
+	} else if (!strnicmp(buf, "disable", 7)) {
+		qpnp_adc_tm_usbid_end();
+		context->id_adc_detect = false;
+		return size;
+	}
+
+	return -EINVAL;
+}
+
+static DEVICE_ATTR(adc_enable, S_IRUGO | S_IWUSR, adc_enable_show,
+		adc_enable_store);
+
 static int __devinit dwc3_msm_probe(struct platform_device *pdev)
 {
 	struct device_node *node = pdev->dev.of_node;
@@ -1662,7 +1935,6 @@
 	int ret = 0;
 	int len = 0;
 	u32 tmp[3];
-	u32 data = 0;
 
 	msm = devm_kzalloc(&pdev->dev, sizeof(*msm), GFP_KERNEL);
 	if (!msm) {
@@ -1677,6 +1949,7 @@
 	INIT_LIST_HEAD(&msm->req_complete_list);
 	INIT_DELAYED_WORK(&msm->chg_work, dwc3_chg_detect_work);
 	INIT_DELAYED_WORK(&msm->resume_work, dwc3_resume_work);
+	INIT_DELAYED_WORK(&msm->init_adc_work, dwc3_init_adc_work);
 
 	msm->xo_handle = msm_xo_get(MSM_XO_TCXO_D0, "usb");
 	if (IS_ERR(msm->xo_handle)) {
@@ -1815,6 +2088,7 @@
 		goto free_hs_ldo_init;
 	}
 
+	msm->ext_xceiv.id = DWC3_ID_FLOAT;
 	msm->ext_xceiv.otg_capability = of_property_read_bool(node,
 				"qcom,otg-capability");
 	msm->charger.charging_disabled = of_property_read_bool(node,
@@ -1835,6 +2109,10 @@
 			}
 			enable_irq_wake(msm->hs_phy_irq);
 		}
+	} else {
+		/* Use ADC for ID pin detection */
+		queue_delayed_work(system_nrt_wq, &msm->init_adc_work, 0);
+		device_create_file(&pdev->dev, &dev_attr_adc_enable);
 	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -1885,40 +2163,7 @@
 	msm->resource_size = resource_size(res);
 	msm->dwc3 = dwc3;
 
-	/* SSPHY Initialization: Use ref_clk from pads and set its parameters */
-	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210002);
-	msleep(30);
-	/* Assert SSPHY reset */
-	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210082);
-	usleep_range(2000, 2200);
-	/* De-assert SSPHY reset - power and ref_clock must be ON */
-	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210002);
-	usleep_range(2000, 2200);
-	/* Ref clock must be stable now, enable ref clock for HS mode */
-	dwc3_msm_write_reg(msm->base, SS_PHY_CTRL_REG, 0x10210102);
-	usleep_range(2000, 2200);
-	/*
-	 * HSPHY Initialization: Enable UTMI clock and clamp enable HVINTs,
-	 * and disable RETENTION (power-on default is ENABLED)
-	 */
-	dwc3_msm_write_reg(msm->base, HS_PHY_CTRL_REG, 0x5220bb2);
-	usleep_range(2000, 2200);
-	/* Disable (bypass) VBUS and ID filters */
-	dwc3_msm_write_reg(msm->base, QSCRATCH_GENERAL_CFG, 0x78);
-
-	/*
-	 * WORKAROUND: There is SSPHY suspend bug due to which USB enumerates
-	 * in HS mode instead of SS mode. Workaround it by asserting
-	 * LANE0.TX_ALT_BLOCK.EN_ALT_BUS to enable TX to use alt bus mode
-	 */
-	data = dwc3_msm_ssusb_read_phycreg(msm->base, 0x102D);
-	data |= (1 << 7);
-	dwc3_msm_ssusb_write_phycreg(msm->base, 0x102D, data);
-
-	data = dwc3_msm_ssusb_read_phycreg(msm->base, 0x1010);
-	data &= ~0xFF0;
-	data |= 0x40;
-	dwc3_msm_ssusb_write_phycreg(msm->base, 0x1010, data);
+	dwc3_msm_qscratch_reg_init(msm);
 
 	pm_runtime_set_active(msm->dev);
 	pm_runtime_enable(msm->dev);
@@ -2001,6 +2246,8 @@
 			goto put_xcvr;
 		}
 
+		if (msm->ext_xceiv.otg_capability)
+			msm->ext_xceiv.ext_block_reset = dwc3_msm_block_reset;
 		ret = dwc3_set_ext_xceiv(msm->otg_xceiv->otg, &msm->ext_xceiv);
 		if (ret || !msm->ext_xceiv.notify_ext_events) {
 			dev_err(&pdev->dev, "failed to register xceiver: %d\n",
@@ -2063,12 +2310,15 @@
 {
 	struct dwc3_msm	*msm = platform_get_drvdata(pdev);
 
+	if (msm->id_adc_detect)
+		qpnp_adc_tm_usbid_end();
 	if (dwc3_debugfs_root)
 		debugfs_remove_recursive(dwc3_debugfs_root);
 	if (msm->otg_xceiv) {
 		dwc3_start_chg_det(&msm->charger, false);
 		usb_put_transceiver(msm->otg_xceiv);
 	}
+
 	pm_runtime_disable(msm->dev);
 	platform_device_unregister(msm->dwc3);
 	wake_lock_destroy(&msm->wlock);
diff --git a/drivers/usb/dwc3/dwc3_otg.c b/drivers/usb/dwc3/dwc3_otg.c
index fab443c..136cc5d 100644
--- a/drivers/usb/dwc3/dwc3_otg.c
+++ b/drivers/usb/dwc3/dwc3_otg.c
@@ -1,7 +1,7 @@
 /**
  * dwc3_otg.c - DesignWare USB3 DRD Controller OTG
  *
- * Copyright (c) 2012, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -38,12 +38,21 @@
  */
 static void dwc3_otg_set_host_regs(struct dwc3_otg *dotg)
 {
-	u32 octl;
+	u32 reg;
+	struct dwc3 *dwc = dotg->dwc;
+	struct dwc3_ext_xceiv *ext_xceiv = dotg->ext_xceiv;
 
-	/* Set OCTL[6](PeriMode) to 0 (host) */
-	octl = dwc3_readl(dotg->regs, DWC3_OCTL);
-	octl &= ~DWC3_OTG_OCTL_PERIMODE;
-	dwc3_writel(dotg->regs, DWC3_OCTL, octl);
+	if (ext_xceiv && !ext_xceiv->otg_capability) {
+		/* Set OCTL[6](PeriMode) to 0 (host) */
+		reg = dwc3_readl(dotg->regs, DWC3_OCTL);
+		reg &= ~DWC3_OTG_OCTL_PERIMODE;
+		dwc3_writel(dotg->regs, DWC3_OCTL, reg);
+	} else {
+		reg = dwc3_readl(dwc->regs, DWC3_GCTL);
+		reg &= ~(DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_OTG));
+		reg |= DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_HOST);
+		dwc3_writel(dwc->regs, DWC3_GCTL, reg);
+	}
 }
 
 /**
@@ -76,17 +85,25 @@
  */
 static void dwc3_otg_set_peripheral_regs(struct dwc3_otg *dotg)
 {
-	u32 octl;
+	u32 reg;
+	struct dwc3 *dwc = dotg->dwc;
+	struct dwc3_ext_xceiv *ext_xceiv = dotg->ext_xceiv;
 
-	/* Set OCTL[6](PeriMode) to 1 (peripheral) */
-	octl = dwc3_readl(dotg->regs, DWC3_OCTL);
-	octl |= DWC3_OTG_OCTL_PERIMODE;
-	dwc3_writel(dotg->regs, DWC3_OCTL, octl);
-
-	/*
-	 * TODO: add more OTG registers writes for PERIPHERAL mode here,
-	 * see figure 12-19 B-device flow in dwc3 Synopsis spec
-	 */
+	if (ext_xceiv && !ext_xceiv->otg_capability) {
+		/* Set OCTL[6](PeriMode) to 1 (peripheral) */
+		reg = dwc3_readl(dotg->regs, DWC3_OCTL);
+		reg |= DWC3_OTG_OCTL_PERIMODE;
+		dwc3_writel(dotg->regs, DWC3_OCTL, reg);
+		/*
+		 * TODO: add more OTG registers writes for PERIPHERAL mode here,
+		 * see figure 12-19 B-device flow in dwc3 Synopsis spec
+		 */
+	} else {
+		reg = dwc3_readl(dwc->regs, DWC3_GCTL);
+		reg &= ~(DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_OTG));
+		reg |= DWC3_GCTL_PRTCAPDIR(DWC3_GCTL_PRTCAP_DEVICE);
+		dwc3_writel(dwc->regs, DWC3_GCTL, reg);
+	}
 }
 
 /**
@@ -100,6 +117,7 @@
 static int dwc3_otg_start_host(struct usb_otg *otg, int on)
 {
 	struct dwc3_otg *dotg = container_of(otg, struct dwc3_otg, otg);
+	struct dwc3_ext_xceiv *ext_xceiv = dotg->ext_xceiv;
 	struct dwc3 *dwc = dotg->dwc;
 	int ret = 0;
 
@@ -148,7 +166,8 @@
 		}
 
 		/* re-init OTG EVTEN register as XHCI reset clears it */
-		dwc3_otg_reset(dotg);
+		if (ext_xceiv && !ext_xceiv->otg_capability)
+			dwc3_otg_reset(dotg);
 	} else {
 		dev_dbg(otg->phy->dev, "%s: turn off host\n", __func__);
 
@@ -161,9 +180,15 @@
 		}
 		dwc3_otg_notify_host_mode(otg, on);
 
+		/* Do block reset for Host <-> peripheral switching to work */
+		if (ext_xceiv && ext_xceiv->otg_capability &&
+						ext_xceiv->ext_block_reset)
+			ext_xceiv->ext_block_reset();
+
 		/* re-init core and OTG register as XHCI reset clears it */
 		dwc3_post_host_reset_core_init(dwc);
-		dwc3_otg_reset(dotg);
+		if (ext_xceiv && !ext_xceiv->otg_capability)
+			dwc3_otg_reset(dotg);
 	}
 
 	return 0;
@@ -312,6 +337,7 @@
 	struct dwc3_ext_xceiv *ext_xceiv = dotg->ext_xceiv;
 	struct usb_phy *phy = dotg->otg.phy;
 	int ret = 0;
+	int work = 0;
 
 	if (event == DWC3_EVENT_PHY_RESUME) {
 		if (!pm_runtime_status_suspended(phy->dev)) {
@@ -331,17 +357,28 @@
 			}
 		}
 	} else if (event == DWC3_EVENT_XCEIV_STATE) {
-		if (ext_xceiv->id == DWC3_ID_FLOAT)
-			set_bit(ID, &dotg->inputs);
-		else
-			clear_bit(ID, &dotg->inputs);
+		if (ext_xceiv->id == DWC3_ID_FLOAT) {
+			if (!test_and_set_bit(ID, &dotg->inputs)) {
+				dev_dbg(phy->dev, "XCVR: ID set\n");
+				work = 1;
+			}
+		} else {
+			if (test_and_clear_bit(ID, &dotg->inputs)) {
+				dev_dbg(phy->dev, "XCVR: ID clear\n");
+				work = 1;
+			}
+		}
 
 		if (ext_xceiv->bsv) {
-			dev_dbg(phy->dev, "XCVR: BSV set\n");
-			set_bit(B_SESS_VLD, &dotg->inputs);
+			if (!test_and_set_bit(B_SESS_VLD, &dotg->inputs)) {
+				dev_dbg(phy->dev, "XCVR: BSV set\n");
+				work = 1;
+			}
 		} else {
-			dev_dbg(phy->dev, "XCVR: BSV clear\n");
-			clear_bit(B_SESS_VLD, &dotg->inputs);
+			if (test_and_clear_bit(B_SESS_VLD, &dotg->inputs)) {
+				dev_dbg(phy->dev, "XCVR: BSV clear\n");
+				work = 1;
+			}
 		}
 
 		if (!init) {
@@ -350,7 +387,8 @@
 			dev_dbg(phy->dev, "XCVR: BSV init complete\n");
 			return;
 		}
-		schedule_work(&dotg->sm_work);
+		if (work)
+			schedule_work(&dotg->sm_work);
 	}
 }
 
@@ -457,6 +495,7 @@
 	u32 osts, oevt_reg;
 	int ret = IRQ_NONE;
 	int handled_irqs = 0;
+	struct usb_phy *phy = dotg->otg.phy;
 
 	oevt_reg = dwc3_readl(dotg->regs, DWC3_OEVT);
 
@@ -472,23 +511,30 @@
 		 * function, switch from A to B or from B to A.
 		 */
 
-		if (osts & DWC3_OTG_OSTS_CONIDSTS)
-			set_bit(ID, &dotg->inputs);
-		else
-			clear_bit(ID, &dotg->inputs);
+		if (oevt_reg & DWC3_OEVTEN_OTGCONIDSTSCHNGEVNT) {
+			if (osts & DWC3_OTG_OSTS_CONIDSTS) {
+				dev_dbg(phy->dev, "ID set\n");
+				set_bit(ID, &dotg->inputs);
+			} else {
+				dev_dbg(phy->dev, "ID clear\n");
+				clear_bit(ID, &dotg->inputs);
+			}
+			handled_irqs |= DWC3_OEVTEN_OTGCONIDSTSCHNGEVNT;
+		}
 
-		if (osts & DWC3_OTG_OSTS_BSESVALID)
-			set_bit(B_SESS_VLD, &dotg->inputs);
-		else
-			clear_bit(B_SESS_VLD, &dotg->inputs);
+		if (oevt_reg & DWC3_OEVTEN_OTGBDEVVBUSCHNGEVNT) {
+			if (osts & DWC3_OTG_OSTS_BSESVALID) {
+				dev_dbg(phy->dev, "BSV set\n");
+				set_bit(B_SESS_VLD, &dotg->inputs);
+			} else {
+				dev_dbg(phy->dev, "BSV clear\n");
+				clear_bit(B_SESS_VLD, &dotg->inputs);
+			}
+			handled_irqs |= DWC3_OEVTEN_OTGBDEVVBUSCHNGEVNT;
+		}
 
 		schedule_work(&dotg->sm_work);
 
-		handled_irqs |= (oevt_reg & DWC3_OEVTEN_OTGCONIDSTSCHNGEVNT) ?
-				DWC3_OEVTEN_OTGCONIDSTSCHNGEVNT : 0;
-		handled_irqs |= (oevt_reg & DWC3_OEVTEN_OTGBDEVVBUSCHNGEVNT) ?
-				DWC3_OEVTEN_OTGBDEVVBUSCHNGEVNT : 0;
-
 		ret = IRQ_HANDLED;
 
 		/* Clear the interrupts we handled */
diff --git a/drivers/usb/dwc3/dwc3_otg.h b/drivers/usb/dwc3/dwc3_otg.h
index c93ce5f..5a36a4f 100644
--- a/drivers/usb/dwc3/dwc3_otg.h
+++ b/drivers/usb/dwc3/dwc3_otg.h
@@ -103,6 +103,8 @@
 	/* to notify OTG about LPM exit event, provided by OTG */
 	void	(*notify_ext_events)(struct usb_otg *otg,
 					enum dwc3_ext_events ext_event);
+	/* for block reset USB core */
+	void	(*ext_block_reset)(void);
 };
 
 /* for external transceiver driver */
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 3679191..c2bc3f3 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -1546,11 +1546,63 @@
 {
 	struct dwc3_ep		*dep;
 	int			ret = 0;
+	u32			reg;
 
-	/* reinitialize physical ep0-1 */
+	/* Enable all but Start and End of Frame IRQs */
+	reg = (DWC3_DEVTEN_EVNTOVERFLOWEN |
+			DWC3_DEVTEN_CMDCMPLTEN |
+			DWC3_DEVTEN_ERRTICERREN |
+			DWC3_DEVTEN_WKUPEVTEN |
+			DWC3_DEVTEN_ULSTCNGEN |
+			DWC3_DEVTEN_CONNECTDONEEN |
+			DWC3_DEVTEN_USBRSTEN |
+			DWC3_DEVTEN_DISCONNEVTEN);
+	dwc3_writel(dwc->regs, DWC3_DEVTEN, reg);
+
+	/* Enable USB2 LPM and automatic phy suspend only on recent versions */
+	if (dwc->revision >= DWC3_REVISION_194A) {
+		reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+		reg |= DWC3_DCFG_LPM_CAP;
+		dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+
+		reg = dwc3_readl(dwc->regs, DWC3_DCTL);
+		reg &= ~(DWC3_DCTL_HIRD_THRES_MASK | DWC3_DCTL_L1_HIBER_EN);
+
+		/* TODO: This should be configurable */
+		reg |= DWC3_DCTL_HIRD_THRES(28);
+
+		dwc3_writel(dwc->regs, DWC3_DCTL, reg);
+	}
+
+	reg = dwc3_readl(dwc->regs, DWC3_DCFG);
+	reg &= ~(DWC3_DCFG_SPEED_MASK);
+
+	/**
+	 * WORKAROUND: DWC3 revision < 2.20a have an issue
+	 * which would cause metastability state on Run/Stop
+	 * bit if we try to force the IP to USB2-only mode.
+	 *
+	 * Because of that, we cannot configure the IP to any
+	 * speed other than the SuperSpeed
+	 *
+	 * Refers to:
+	 *
+	 * STAR#9000525659: Clock Domain Crossing on DCTL in
+	 * USB 2.0 Mode
+	 */
+	if (dwc->revision < DWC3_REVISION_220A)
+		reg |= DWC3_DCFG_SUPERSPEED;
+	else
+		reg |= dwc->maximum_speed;
+	dwc3_writel(dwc->regs, DWC3_DCFG, reg);
+
+	dwc->start_config_issued = false;
+
+	/* Start with SuperSpeed Default */
+	dwc3_gadget_ep0_desc.wMaxPacketSize = cpu_to_le16(512);
 
 	dwc->delayed_status = false;
-
+	/* reinitialize physical ep0-1 */
 	dep = dwc->eps[0];
 	dep->flags = 0;
 	ret = __dwc3_gadget_ep_enable(dep, &dwc3_gadget_ep0_desc, NULL, false);
diff --git a/drivers/usb/gadget/android.c b/drivers/usb/gadget/android.c
index b773d1a..fe59036 100644
--- a/drivers/usb/gadget/android.c
+++ b/drivers/usb/gadget/android.c
@@ -75,6 +75,7 @@
 #include "rndis.c"
 #include "u_bam_data.c"
 #include "f_mbim.c"
+#include "f_ecm.c"
 #include "f_qc_ecm.c"
 #include "f_qc_rndis.c"
 #include "u_ether.c"
@@ -1349,6 +1350,50 @@
 	.attributes	= rndis_function_attributes,
 };
 
+static int ecm_function_bind_config(struct android_usb_function *f,
+					struct usb_configuration *c)
+{
+	int ret;
+	struct ecm_function_config *ecm = f->config;
+
+	if (!ecm) {
+		pr_err("%s: ecm_pdata\n", __func__);
+		return -EINVAL;
+	}
+
+	pr_info("%s MAC: %02X:%02X:%02X:%02X:%02X:%02X\n", __func__,
+		ecm->ethaddr[0], ecm->ethaddr[1], ecm->ethaddr[2],
+		ecm->ethaddr[3], ecm->ethaddr[4], ecm->ethaddr[5]);
+
+	ret = gether_setup_name(c->cdev->gadget, ecm->ethaddr, "ecm");
+	if (ret) {
+		pr_err("%s: gether_setup failed\n", __func__);
+		return ret;
+	}
+
+	ret = ecm_bind_config(c, ecm->ethaddr);
+	if (ret) {
+		pr_err("%s: ecm_bind_config failed\n", __func__);
+		gether_cleanup();
+	}
+	return ret;
+}
+
+static void ecm_function_unbind_config(struct android_usb_function *f,
+						struct usb_configuration *c)
+{
+	gether_cleanup();
+}
+
+static struct android_usb_function ecm_function = {
+	.name		= "ecm",
+	.init		= ecm_function_init,
+	.cleanup	= ecm_function_cleanup,
+	.bind_config	= ecm_function_bind_config,
+	.unbind_config	= ecm_function_unbind_config,
+	.attributes	= ecm_function_attributes,
+};
+
 struct mass_storage_function_config {
 	struct fsg_config fsg;
 	struct fsg_common *common;
@@ -1618,6 +1663,7 @@
 	&ptp_function,
 	&rndis_function,
 	&rndis_qc_function,
+	&ecm_function,
 	&mass_storage_function,
 	&accessory_function,
 	&audio_source_function,
diff --git a/drivers/usb/gadget/f_mbim.c b/drivers/usb/gadget/f_mbim.c
index 85240ef..65b4890 100644
--- a/drivers/usb/gadget/f_mbim.c
+++ b/drivers/usb/gadget/f_mbim.c
@@ -216,7 +216,7 @@
 	.bcdMbbVersion =	cpu_to_le16(0x0100),
 
 	.wMaxControlMessage =	cpu_to_le16(0x1000),
-	.bNumberFilters =	0x10,
+	.bNumberFilters =	0x20,
 	.bMaxFilterSize =	0x80,
 	.wMaxSegmentSize =	cpu_to_le16(0xfe0),
 	.bmNetworkCapabilities = 0x20,
diff --git a/drivers/usb/gadget/f_rmnet.c b/drivers/usb/gadget/f_rmnet.c
index 79aac27..32fc79e 100644
--- a/drivers/usb/gadget/f_rmnet.c
+++ b/drivers/usb/gadget/f_rmnet.c
@@ -50,9 +50,6 @@
 	struct list_head		cpkt_resp_q;
 	atomic_t			notify_count;
 	unsigned long			cpkts_len;
-
-	/* IPA / RmNet Bridge support*/
-	struct usb_bam_connect_ipa_params ipa_params;
 };
 
 #define NR_RMNET_PORTS	3
@@ -433,18 +430,9 @@
 	switch (dxport) {
 	case USB_GADGET_XPORT_BAM:
 	case USB_GADGET_XPORT_BAM2BAM:
-		ret = gbam_connect(&dev->port, port_num,
-						   dxport, port_num, NULL);
-		if (ret) {
-			pr_err("%s: gbam_connect failed: err:%d\n",
-					__func__, ret);
-			gsmd_ctrl_disconnect(&dev->port, port_num);
-			return ret;
-		}
-		break;
 	case USB_GADGET_XPORT_BAM2BAM_IPA:
 		ret = gbam_connect(&dev->port, port_num,
-					dxport, port_num, &(dev->ipa_params));
+						   dxport, port_num);
 		if (ret) {
 			pr_err("%s: gbam_connect failed: err:%d\n",
 					__func__, ret);
@@ -514,11 +502,8 @@
 	switch (dxport) {
 	case USB_GADGET_XPORT_BAM:
 	case USB_GADGET_XPORT_BAM2BAM:
-		gbam_disconnect(&dev->port, port_num, dxport, NULL);
-		break;
 	case USB_GADGET_XPORT_BAM2BAM_IPA:
-		gbam_disconnect(&dev->port, port_num, dxport,
-						&(dev->ipa_params));
+		gbam_disconnect(&dev->port, port_num, dxport);
 		break;
 	case USB_GADGET_XPORT_HSIC:
 		ghsic_data_disconnect(&dev->port, port_num);
diff --git a/drivers/usb/gadget/f_rndis.c b/drivers/usb/gadget/f_rndis.c
index 02f044e..288611d 100644
--- a/drivers/usb/gadget/f_rndis.c
+++ b/drivers/usb/gadget/f_rndis.c
@@ -840,7 +840,6 @@
 
 	rndis_deregister(rndis->config);
 	rndis_exit();
-	rndis_string_defs[0].id = 0;
 
 	if (gadget_is_superspeed(c->cdev->gadget))
 		usb_free_descriptors(f->ss_descriptors);
diff --git a/drivers/usb/gadget/u_bam.c b/drivers/usb/gadget/u_bam.c
index 7f3713f..aa93a7d 100644
--- a/drivers/usb/gadget/u_bam.c
+++ b/drivers/usb/gadget/u_bam.c
@@ -102,7 +102,7 @@
 	u32					dst_pipe_idx;
 	u8					connection_idx;
 	enum transport_type trans;
-	struct usb_bam_connect_ipa_params *ipa_params;
+	struct usb_bam_connect_ipa_params ipa_params;
 
 	/* stats */
 	unsigned int		pending_with_bam;
@@ -649,7 +649,7 @@
 	int ret;
 
 	if (d->trans == USB_GADGET_XPORT_BAM2BAM_IPA) {
-		ret = usb_bam_disconnect_ipa(d->connection_idx, d->ipa_params);
+		ret = usb_bam_disconnect_ipa(d->connection_idx, &d->ipa_params);
 		if (ret)
 			pr_err("%s: usb_bam_disconnect_ipa failed: err:%d\n",
 				__func__, ret);
@@ -706,29 +706,29 @@
 			return;
 		}
 	} else if (d->trans == USB_GADGET_XPORT_BAM2BAM_IPA) {
-		d->ipa_params->client = IPA_CLIENT_USB_CONS;
-		d->ipa_params->dir = PEER_PERIPHERAL_TO_USB;
-		ret = usb_bam_connect_ipa(d->ipa_params);
+		d->ipa_params.client = IPA_CLIENT_USB_CONS;
+		d->ipa_params.dir = PEER_PERIPHERAL_TO_USB;
+		ret = usb_bam_connect_ipa(&d->ipa_params);
 		if (ret) {
 			pr_err("%s: usb_bam_connect_ipa failed: err:%d\n",
 				__func__, ret);
 			return;
 		}
 
-		d->ipa_params->client = IPA_CLIENT_USB_PROD;
-		d->ipa_params->dir = USB_TO_PEER_PERIPHERAL;
+		d->ipa_params.client = IPA_CLIENT_USB_PROD;
+		d->ipa_params.dir = USB_TO_PEER_PERIPHERAL;
 		/* Currently only DMA mode is supported */
-		d->ipa_params->ipa_ep_cfg.mode.mode = IPA_DMA;
-		d->ipa_params->ipa_ep_cfg.mode.dst =
+		d->ipa_params.ipa_ep_cfg.mode.mode = IPA_DMA;
+		d->ipa_params.ipa_ep_cfg.mode.dst =
 				IPA_CLIENT_A2_TETHERED_CONS;
-		ret = usb_bam_connect_ipa(d->ipa_params);
+		ret = usb_bam_connect_ipa(&d->ipa_params);
 		if (ret) {
 			pr_err("%s: usb_bam_connect_ipa failed: err:%d\n",
 				__func__, ret);
 			return;
 		}
-		rmnet_bridge_connect(d->ipa_params->prod_clnt_hdl,
-				d->ipa_params->cons_clnt_hdl, 0);
+		rmnet_bridge_connect(d->ipa_params.prod_clnt_hdl,
+				d->ipa_params.cons_clnt_hdl, 0);
 	}
 
 	d->rx_req = usb_ep_alloc_request(port->port_usb->out, GFP_KERNEL);
@@ -1037,8 +1037,7 @@
 static void gam_debugfs_init(void) { }
 #endif
 
-void gbam_disconnect(struct grmnet *gr, u8 port_num, enum transport_type trans,
-			struct usb_bam_connect_ipa_params *ipa_params)
+void gbam_disconnect(struct grmnet *gr, u8 port_num, enum transport_type trans)
 {
 	struct gbam_port	*port;
 	unsigned long		flags;
@@ -1096,8 +1095,7 @@
 }
 
 int gbam_connect(struct grmnet *gr, u8 port_num,
-				 enum transport_type trans, u8 connection_idx,
-				 struct usb_bam_connect_ipa_params *ipa_params)
+				 enum transport_type trans, u8 connection_idx)
 {
 	struct gbam_port	*port;
 	struct bam_ch_info	*d;
@@ -1166,11 +1164,10 @@
 		port->gr = gr;
 		d->connection_idx = connection_idx;
 	} else if (trans == USB_GADGET_XPORT_BAM2BAM_IPA) {
-		d->ipa_params = ipa_params;
 		port->gr = gr;
-		d->ipa_params->src_pipe = &(d->src_pipe_idx);
-		d->ipa_params->dst_pipe = &(d->dst_pipe_idx);
-		d->ipa_params->idx = connection_idx;
+		d->ipa_params.src_pipe = &(d->src_pipe_idx);
+		d->ipa_params.dst_pipe = &(d->dst_pipe_idx);
+		d->ipa_params.idx = connection_idx;
 	}
 
 	d->trans = trans;
diff --git a/drivers/usb/gadget/u_rmnet.h b/drivers/usb/gadget/u_rmnet.h
index a3d42fa..0f7c4fb 100644
--- a/drivers/usb/gadget/u_rmnet.h
+++ b/drivers/usb/gadget/u_rmnet.h
@@ -48,10 +48,8 @@
 
 int gbam_setup(unsigned int no_bam_port, unsigned int no_bam2bam_port);
 int gbam_connect(struct grmnet *gr, u8 port_num,
-				 enum transport_type trans, u8 connection_idx,
-				 struct usb_bam_connect_ipa_params *ipa_params);
-void gbam_disconnect(struct grmnet *gr, u8 port_num, enum transport_type trans,
-				struct usb_bam_connect_ipa_params *ipa_params);
+				 enum transport_type trans, u8 connection_idx);
+void gbam_disconnect(struct grmnet *gr, u8 port_num, enum transport_type trans);
 void gbam_suspend(struct grmnet *gr, u8 port_num, enum transport_type trans);
 void gbam_resume(struct grmnet *gr, u8 port_num, enum transport_type trans);
 int gsmd_ctrl_connect(struct grmnet *gr, int port_num);
diff --git a/drivers/usb/host/ehci-msm-hsic.c b/drivers/usb/host/ehci-msm-hsic.c
index 7d12598..57598c8 100644
--- a/drivers/usb/host/ehci-msm-hsic.c
+++ b/drivers/usb/host/ehci-msm-hsic.c
@@ -31,9 +31,11 @@
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
 
+#include <linux/usb/ulpi.h>
 #include <linux/usb/msm_hsusb_hw.h>
 #include <linux/usb/msm_hsusb.h>
 #include <linux/gpio.h>
+#include <linux/of_gpio.h>
 #include <linux/spinlock.h>
 #include <linux/irq.h>
 #include <linux/kthread.h>
@@ -106,6 +108,7 @@
 static bool debug_bus_voting_enabled = true;
 static u64 ehci_msm_hsic_dma_mask = DMA_BIT_MASK(32);
 
+static struct platform_driver ehci_msm_hsic_driver;
 
 static unsigned int enable_payload_log = 1;
 module_param(enable_payload_log, uint, S_IRUGO | S_IWUSR);
@@ -611,15 +614,17 @@
 {
 	struct msm_hsic_host_platform_data *pdata = mehci->dev->platform_data;
 	int ret;
+	void __iomem *reg;
 
 	/* HSIC init sequence when HSIC signals (Strobe/Data) are
 	routed via GPIOs */
 	if (pdata && pdata->strobe && pdata->data) {
 
-		/* Enable LV_MODE in HSIC_CAL_PAD_CTL register */
-		writel_relaxed(HSIC_LV_MODE, HSIC_CAL_PAD_CTL);
-
-		mb();
+		if (!pdata->ignore_cal_pad_config) {
+			/* Enable LV_MODE in HSIC_CAL_PAD_CTL register */
+			writel_relaxed(HSIC_LV_MODE, HSIC_CAL_PAD_CTL);
+			mb();
+		}
 
 		/*set periodic calibration interval to ~2.048sec in
 		  HSIC_IO_CAL_REG */
@@ -634,9 +639,25 @@
 			dev_err(mehci->dev, " gpio configuarion failed\n");
 			return ret;
 		}
-		/* Set LV_MODE=0x1 and DCC=0x2 in HSIC_GPIO PAD_CTL register */
-		writel_relaxed(HSIC_GPIO_PAD_VAL, HSIC_STROBE_GPIO_PAD_CTL);
-		writel_relaxed(HSIC_GPIO_PAD_VAL, HSIC_DATA_GPIO_PAD_CTL);
+		if (pdata->strobe_pad_offset) {
+			/* Set CORE_CTL_EN in STROBE GPIO PAD_CTL register */
+			reg = MSM_TLMM_BASE + pdata->strobe_pad_offset;
+			writel_relaxed(readl_relaxed(reg) | 0x2000000, reg);
+		} else {
+			/* Set LV_MODE=0x1 and DCC=0x2 in STROBE GPIO PAD_CTL */
+			reg = HSIC_STROBE_GPIO_PAD_CTL;
+			writel_relaxed(HSIC_GPIO_PAD_VAL, reg);
+		}
+
+		if (pdata->data_pad_offset) {
+			/* Set CORE_CTL_EN in HSIC_DATA GPIO PAD_CTL register */
+			reg = MSM_TLMM_BASE + pdata->data_pad_offset;
+			writel_relaxed(readl_relaxed(reg) | 0x2000000, reg);
+		} else {
+			/* Set LV_MODE=0x1 and DCC=0x2 in STROBE GPIO PAD_CTL */
+			reg = HSIC_DATA_GPIO_PAD_CTL;
+			writel_relaxed(HSIC_GPIO_PAD_VAL, reg);
+		}
 
 		mb();
 
@@ -1268,11 +1289,8 @@
 	/* alt_core_clk is for LINK to be used during PHY RESET
 	 * clock rate appropriately set by target specific clock driver */
 	mehci->alt_core_clk = clk_get(mehci->dev, "alt_core_clk");
-	if (IS_ERR(mehci->alt_core_clk)) {
-		dev_err(mehci->dev, "failed to core_clk\n");
-		ret = PTR_ERR(mehci->alt_core_clk);
-		goto put_core_clk;
-	}
+	if (IS_ERR(mehci->alt_core_clk))
+		dev_dbg(mehci->dev, "failed to get alt_core_clk\n");
 
 	/* phy_clk is required for HSIC PHY operation
 	 * clock rate appropriately set by target specific clock driver */
@@ -1290,7 +1308,6 @@
 		ret = PTR_ERR(mehci->cal_clk);
 		goto put_phy_clk;
 	}
-	clk_set_rate(mehci->cal_clk, 10000000);
 
 	/* ahb_clk is required for data transfers */
 	mehci->ahb_clk = clk_get(mehci->dev, "iface_clk");
@@ -1320,8 +1337,8 @@
 put_phy_clk:
 	clk_put(mehci->phy_clk);
 put_alt_core_clk:
-	clk_put(mehci->alt_core_clk);
-put_core_clk:
+	if (!IS_ERR(mehci->alt_core_clk))
+		clk_put(mehci->alt_core_clk);
 	clk_put(mehci->core_clk);
 
 	return ret;
@@ -1569,6 +1586,36 @@
 	debugfs_remove_recursive(ehci_hsic_msm_dbg_root);
 }
 
+struct msm_hsic_host_platform_data *msm_hsic_dt_to_pdata(
+				struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct msm_hsic_host_platform_data *pdata;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata) {
+		dev_err(&pdev->dev, "unable to allocate platform data\n");
+		return NULL;
+	}
+	pdata->strobe = of_get_named_gpio(node, "hsic,strobe-gpio", 0);
+	if (pdata->strobe < 0)
+		pdata->strobe = 0;
+
+	pdata->data = of_get_named_gpio(node, "hsic,data-gpio", 0);
+	if (pdata->data < 0)
+		pdata->data = 0;
+
+	pdata->ignore_cal_pad_config = of_property_read_bool(node,
+					"hsic,ignore-cal-pad-config");
+	of_property_read_u32(node, "hsic,strobe-pad-offset",
+					&pdata->strobe_pad_offset);
+	of_property_read_u32(node, "hsic,data-pad-offset",
+					&pdata->data_pad_offset);
+
+	return pdata;
+}
+
+
 static int __devinit ehci_hsic_msm_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd;
@@ -1579,6 +1626,14 @@
 
 	dev_dbg(&pdev->dev, "ehci_msm-hsic probe\n");
 
+	if (pdev->dev.of_node) {
+		dev_dbg(&pdev->dev, "device tree enabled\n");
+		pdev->dev.platform_data = msm_hsic_dt_to_pdata(pdev);
+		dev_set_name(&pdev->dev, ehci_msm_hsic_driver.driver.name);
+	}
+	if (!pdev->dev.platform_data)
+		dev_dbg(&pdev->dev, "No platform data given\n");
+
 	if (!pdev->dev.dma_mask)
 		pdev->dev.dma_mask = &ehci_msm_hsic_dma_mask;
 	if (!pdev->dev.coherent_dma_mask)
diff --git a/drivers/usb/misc/diag_bridge.c b/drivers/usb/misc/diag_bridge.c
index 2d95945..ae7e1b6 100644
--- a/drivers/usb/misc/diag_bridge.c
+++ b/drivers/usb/misc/diag_bridge.c
@@ -527,6 +527,8 @@
 	.driver_info = VALID_INTERFACE_NUM, },
 	{ USB_DEVICE(0x5c6, 0x904C),
 	.driver_info = VALID_INTERFACE_NUM, },
+	{ USB_DEVICE(0x5c6, 0x9075),
+	.driver_info = VALID_INTERFACE_NUM, },
 
 	{} /* terminating entry */
 };
diff --git a/drivers/usb/misc/ks_bridge.c b/drivers/usb/misc/ks_bridge.c
index 410b5c4..86c59e7 100644
--- a/drivers/usb/misc/ks_bridge.c
+++ b/drivers/usb/misc/ks_bridge.c
@@ -402,6 +402,8 @@
 	.driver_info = (unsigned long)&ksb_efs_dev, },
 	{ USB_DEVICE(0x5c6, 0x904C),
 	.driver_info = (unsigned long)&ksb_efs_dev, },
+	{ USB_DEVICE(0x5c6, 0x9075),
+	.driver_info = (unsigned long)&ksb_efs_dev, },
 
 	{} /* terminating entry */
 };
diff --git a/drivers/usb/misc/mdm_data_bridge.c b/drivers/usb/misc/mdm_data_bridge.c
index e821fda..655e2f6 100644
--- a/drivers/usb/misc/mdm_data_bridge.c
+++ b/drivers/usb/misc/mdm_data_bridge.c
@@ -1017,6 +1017,7 @@
 #define PID9034_IFACE_MASK	0xC
 #define PID9048_IFACE_MASK	0x18
 #define PID904C_IFACE_MASK	0x28
+#define PID9075_IFACE_MASK	0x28
 
 static const struct usb_device_id bridge_ids[] = {
 	{ USB_DEVICE(0x5c6, 0x9001),
@@ -1031,6 +1032,9 @@
 	{ USB_DEVICE(0x5c6, 0x904c),
 	.driver_info = PID904C_IFACE_MASK,
 	},
+	{ USB_DEVICE(0x5c6, 0x9075),
+	.driver_info = PID9075_IFACE_MASK,
+	},
 
 	{ } /* Terminating entry */
 };
diff --git a/drivers/usb/otg/msm_otg.c b/drivers/usb/otg/msm_otg.c
index dde9312..3ad05b06 100644
--- a/drivers/usb/otg/msm_otg.c
+++ b/drivers/usb/otg/msm_otg.c
@@ -3133,8 +3133,16 @@
 	struct msm_otg *motg = the_msm_otg;
 
 	/* Ignore received BSV interrupts, if ID pin is GND */
-	if (!test_bit(ID, &motg->inputs))
-		return;
+	if (!test_bit(ID, &motg->inputs)) {
+		/*
+		 * state machine work waits for initial VBUS
+		 * completion in UNDEFINED state.  Process
+		 * the initial VBUS event in ID_GND state.
+		 */
+		if (init)
+			return;
+		goto complete;
+	}
 
 	if (online) {
 		pr_debug("PMIC: BSV set\n");
@@ -3143,7 +3151,7 @@
 		pr_debug("PMIC: BSV clear\n");
 		clear_bit(B_SESS_VLD, &motg->inputs);
 	}
-
+complete:
 	if (!init) {
 		init = true;
 		complete(&pmic_vbus_init);
diff --git a/drivers/usb/serial/csvt.c b/drivers/usb/serial/csvt.c
index 3efdd77..6835ddc 100644
--- a/drivers/usb/serial/csvt.c
+++ b/drivers/usb/serial/csvt.c
@@ -48,6 +48,7 @@
 
 static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x05c6 , 0x904c, 0xff, 0xfe, 0xff)},
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x05c6 , 0x9075, 0xff, 0xfe, 0xff)},
 	{}, /* terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, id_table);
diff --git a/drivers/video/msm/hdmi_msm.c b/drivers/video/msm/hdmi_msm.c
index 516c92c..bd32b6d 100644
--- a/drivers/video/msm/hdmi_msm.c
+++ b/drivers/video/msm/hdmi_msm.c
@@ -57,8 +57,8 @@
 #define HDCP_DDC_CTRL_1		0x0124
 #define HDMI_DDC_CTRL		0x020C
 
-#define HPD_DISCONNECT_POLARITY	0
-#define HPD_CONNECT_POLARITY	1
+#define HPD_EVENT_OFFLINE 0
+#define HPD_EVENT_ONLINE  1
 
 #define SWITCH_SET_HDMI_AUDIO(d, force) \
 	do {\
@@ -770,6 +770,14 @@
 static int hdmi_msm_read_edid(void);
 static void hdmi_msm_hpd_off(void);
 
+static bool hdmi_ready(void)
+{
+	return MSM_HDMI_BASE &&
+			hdmi_msm_state &&
+				hdmi_msm_state->hdmi_app_clk &&
+					hdmi_msm_state->hpd_initialized;
+}
+
 static void hdmi_msm_send_event(boolean on)
 {
 	char *envp[2];
@@ -806,12 +814,14 @@
 		kobject_uevent(external_common_state->uevent_kobj,
 			KOBJ_OFFLINE);
 	}
+
+	if (!completion_done(&hdmi_msm_state->hpd_event_processed))
+		complete(&hdmi_msm_state->hpd_event_processed);
 }
 
 static void hdmi_msm_hpd_state_work(struct work_struct *work)
 {
-	if (!hdmi_msm_state || !hdmi_msm_state->hpd_initialized ||
-		!MSM_HDMI_BASE) {
+	if (!hdmi_ready()) {
 		DEV_ERR("hdmi: %s: ignored, probe failed\n", __func__);
 		return;
 	}
@@ -1001,8 +1011,7 @@
 	static uint32 sample_drop_int_occurred;
 	const uint32 occurrence_limit = 5;
 
-	if (!hdmi_msm_state || !hdmi_msm_state->hpd_initialized ||
-		!MSM_HDMI_BASE) {
+	if (!hdmi_ready()) {
 		DEV_DBG("ISR ignored, probe failed\n");
 		return IRQ_HANDLED;
 	}
@@ -4167,19 +4176,28 @@
 }
 #endif
 
-static void hdmi_msm_hpd_polarity_setup(bool polarity, bool trigger)
+static void hdmi_msm_hpd_polarity_setup(void)
 {
 	u32 cable_sense;
+	bool polarity = !external_common_state->hpd_state;
+	bool trigger = false;
+
 	if (polarity)
 		HDMI_OUTP(0x0254, BIT(2) | BIT(1));
 	else
 		HDMI_OUTP(0x0254, BIT(2));
 
 	cable_sense = (HDMI_INP(0x0250) & BIT(1)) >> 1;
-	DEV_DBG("%s: listen=%s, sense=%s\n", __func__,
+
+	if (cable_sense == polarity)
+		trigger = true;
+
+	DEV_DBG("%s: listen=%s, sense=%s, trigger=%s\n", __func__,
 		polarity ? "connect" : "disconnect",
-		cable_sense ? "connect" : "disconnect");
-	if (trigger && (cable_sense == polarity)) {
+		cable_sense ? "connect" : "disconnect",
+		trigger ? "Yes" : "No");
+
+	if (trigger) {
 		u32 reg_val = HDMI_INP(0x0258);
 
 		/* Toggle HPD circuit to trigger HPD sense */
@@ -4280,8 +4298,8 @@
 		/* Turn on HPD HW circuit */
 		HDMI_OUTP(0x0258, hpd_ctrl | BIT(28));
 
-		/* Set up HPD_CTRL to sense HPD event */
-		hdmi_msm_hpd_polarity_setup(HPD_CONNECT_POLARITY, true);
+		/* Set HPD cable sense polarity */
+		hdmi_msm_hpd_polarity_setup();
 	}
 
 	DEV_DBG("%s: (IRQ, 5V on)\n", __func__);
@@ -4297,7 +4315,8 @@
 
 static int hdmi_msm_power_ctrl(boolean enable)
 {
-	int rc = 0;
+	int rc   = 0;
+	int time = 0;
 
 	if (enable) {
 		/*
@@ -4307,7 +4326,22 @@
 		if (hdmi_prim_display ||
 			external_common_state->hpd_feature_on) {
 			DEV_DBG("%s: Turning HPD ciruitry on\n", __func__);
+
 			rc = hdmi_msm_hpd_on();
+			if (rc) {
+				DEV_ERR("%s: HPD ON FAILED\n", __func__);
+				return rc;
+			}
+
+			/* Wait for HPD initialization to complete */
+			INIT_COMPLETION(hdmi_msm_state->hpd_event_processed);
+			time = wait_for_completion_interruptible_timeout(
+				&hdmi_msm_state->hpd_event_processed, HZ);
+			if (!time && !external_common_state->hpd_state) {
+				DEV_DBG("%s: cable not detected\n", __func__);
+				queue_work(hdmi_work_queue,
+				    &hdmi_msm_state->hpd_state_work);
+			}
 		}
 	} else {
 		DEV_DBG("%s: Turning HPD ciruitry off\n", __func__);
@@ -4320,32 +4354,33 @@
 static int hdmi_msm_power_on(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd = platform_get_drvdata(pdev);
+	int ret = 0;
 	bool changed;
 
-	if (!hdmi_msm_state || !hdmi_msm_state->hdmi_app_clk || !MSM_HDMI_BASE)
-		return -ENODEV;
-
-	if (!hdmi_msm_state->hpd_initialized ||
-		!external_common_state->hpd_state) {
-		DEV_DBG("%s: HPD not initialized/cable not conn. Returning\n",
-				__func__);
-		return 0;
+	if (!hdmi_ready()) {
+		DEV_ERR("%s: HDMI/HPD not initialized\n", __func__);
+		return ret;
 	}
 
-	DEV_INFO("power: ON (%dx%d %d)\n", mfd->var_xres, mfd->var_yres,
-		mfd->var_pixclock);
+	if (!external_common_state->hpd_state) {
+		DEV_DBG("%s:HDMI cable not connected\n", __func__);
+		goto error;
+	}
 
 	/* Only start transmission with supported resolution */
 	changed = hdmi_common_get_video_format_from_drv_data(mfd);
 	if (changed || external_common_state->default_res_supported) {
-		hdmi_msm_audio_info_setup(TRUE, 0, 0, 0, FALSE);
 		mutex_lock(&external_common_state_hpd_mutex);
-		hdmi_msm_state->panel_power_on = TRUE;
 		if (external_common_state->hpd_state &&
 				hdmi_msm_is_power_on()) {
-			DEV_DBG("%s: Turning HDMI on\n", __func__);
 			mutex_unlock(&external_common_state_hpd_mutex);
+
+			DEV_INFO("HDMI cable connected %s(%dx%d, %d)\n",
+				__func__, mfd->var_xres, mfd->var_yres,
+				mfd->var_pixclock);
+
 			hdmi_msm_turn_on();
+			hdmi_msm_state->panel_power_on = TRUE;
 
 			if (hdmi_msm_state->hdcp_enable) {
 				/* Kick off HDCP Authentication */
@@ -4370,10 +4405,11 @@
 				external_common_state->video_resolution);
 	}
 
-	/* Enable HPD interrupt and listen to disconnect interrupts */
-	hdmi_msm_hpd_polarity_setup(HPD_DISCONNECT_POLARITY,
-			external_common_state->hpd_state);
-	return 0;
+error:
+	/* Set HPD cable sense polarity */
+	hdmi_msm_hpd_polarity_setup();
+
+	return ret;
 }
 
 void mhl_connect_api(boolean on)
@@ -4422,12 +4458,16 @@
  */
 static int hdmi_msm_power_off(struct platform_device *pdev)
 {
-	if (!hdmi_msm_state->hdmi_app_clk)
-		return -ENODEV;
+	int ret = 0;
+
+	if (!hdmi_ready()) {
+		DEV_ERR("%s: HDMI/HPD not initialized\n", __func__);
+		return ret;
+	}
 
 	if (!hdmi_msm_state->panel_power_on) {
-		DEV_DBG("%s: panel not on. returning\n", __func__);
-		return 0;
+		DEV_DBG("%s: panel not ON\n", __func__);
+		goto error;
 	}
 
 	if (hdmi_msm_state->hdcp_enable) {
@@ -4463,11 +4503,13 @@
 	hdmi_msm_state->panel_power_on = FALSE;
 	DEV_INFO("power: OFF (audio off)\n");
 
-	/* Enable HPD interrupt and listen to connect interrupts */
-	hdmi_msm_hpd_polarity_setup(HPD_CONNECT_POLARITY,
-				!external_common_state->hpd_state);
+	if (!completion_done(&hdmi_msm_state->hpd_event_processed))
+		complete(&hdmi_msm_state->hpd_event_processed);
+error:
+	/* Set HPD cable sense polarity */
+	hdmi_msm_hpd_polarity_setup();
 
-	return 0;
+	return ret;
 }
 
 bool mhl_is_enabled(void)
@@ -4730,9 +4772,19 @@
 	if (on) {
 		rc = hdmi_msm_hpd_on();
 	} else {
-		external_common_state->hpd_state = 0;
+		if (external_common_state->hpd_state) {
+			external_common_state->hpd_state = 0;
+
+			/* Send offline event to switch OFF HDMI and HAL FD */
+			hdmi_msm_send_event(HPD_EVENT_OFFLINE);
+
+			/* Wait for HDMI and FD to close */
+			INIT_COMPLETION(hdmi_msm_state->hpd_event_processed);
+			wait_for_completion_interruptible_timeout(
+				&hdmi_msm_state->hpd_event_processed, HZ);
+		}
+
 		hdmi_msm_hpd_off();
-		SWITCH_SET_HDMI_AUDIO(0, 0);
 
 		/* Set HDMI switch node to 0 on HPD feature disable */
 		switch_set_state(&external_common_state->sdev, 0);
@@ -4828,6 +4880,7 @@
 
 	hdmi_common_init_panel_info(&hdmi_msm_panel_data.panel_info);
 	init_completion(&hdmi_msm_state->ddc_sw_done);
+	init_completion(&hdmi_msm_state->hpd_event_processed);
 	INIT_WORK(&hdmi_msm_state->hpd_state_work, hdmi_msm_hpd_state_work);
 
 #ifdef CONFIG_FB_MSM_HDMI_MSM_PANEL_CEC_SUPPORT
diff --git a/drivers/video/msm/hdmi_msm.h b/drivers/video/msm/hdmi_msm.h
index 17cefdd..aded4e0 100644
--- a/drivers/video/msm/hdmi_msm.h
+++ b/drivers/video/msm/hdmi_msm.h
@@ -106,6 +106,7 @@
 
 	struct external_common_state_type common;
 	boolean is_mhl_enabled;
+	struct completion hpd_event_processed;
 };
 
 extern struct hdmi_msm_state_type *hdmi_msm_state;
diff --git a/drivers/video/msm/lcdc.c b/drivers/video/msm/lcdc.c
index 2170abe..1bd4302 100644
--- a/drivers/video/msm/lcdc.c
+++ b/drivers/video/msm/lcdc.c
@@ -84,8 +84,6 @@
 		}
 		clk_disable_unprepare(mfd->ebi1_clk);
 	}
-#else
-	mdp_bus_scale_update_request(0);
 #endif
 
 	return ret;
@@ -108,9 +106,7 @@
 
 	if (!panel_pixclock_freq)
 		panel_pixclock_freq = mfd->fbi->var.pixclock;
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(2);
-#else
+#ifndef CONFIG_MSM_BUS_SCALING
 	if (panel_pixclock_freq > 65000000)
 		/* pm_qos_rate should be in Khz */
 		pm_qos_rate = panel_pixclock_freq / 1000 ;
diff --git a/drivers/video/msm/lvds.c b/drivers/video/msm/lvds.c
index 13bb9e3..e9bbceb 100644
--- a/drivers/video/msm/lvds.c
+++ b/drivers/video/msm/lvds.c
@@ -254,10 +254,6 @@
 	if (lvds_pdata && lvds_pdata->lcdc_gpio_config)
 		ret = lvds_pdata->lcdc_gpio_config(0);
 
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(0);
-#endif
-
 	return ret;
 }
 
@@ -273,9 +269,6 @@
 
 	if (!panel_pixclock_freq)
 		panel_pixclock_freq = mfd->fbi->var.pixclock;
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(2);
-#endif
 	mfd = platform_get_drvdata(pdev);
 
 	if (lvds_clk) {
diff --git a/drivers/video/msm/mddi.c b/drivers/video/msm/mddi.c
index 1154913..b4c1e76 100644
--- a/drivers/video/msm/mddi.c
+++ b/drivers/video/msm/mddi.c
@@ -212,9 +212,7 @@
 
 	if (mddi_pdata && mddi_pdata->mddi_power_save)
 		mddi_pdata->mddi_power_save(0);
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(0);
-#else
+#ifndef CONFIG_MSM_BUS_SCALING
 	if (mfd->ebi1_clk)
 		clk_disable_unprepare(mfd->ebi1_clk);
 #endif
@@ -275,9 +273,7 @@
 		printk(KERN_ERR "%s: clk_set_rate failed\n",
 			__func__);
 
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(2);
-#else
+#ifndef CONFIG_MSM_BUS_SCALING
 	if (mfd->ebi1_clk)
 		clk_prepare_enable(mfd->ebi1_clk);
 #endif
diff --git a/drivers/video/msm/mdp.c b/drivers/video/msm/mdp.c
index 372122c..e41017d 100644
--- a/drivers/video/msm/mdp.c
+++ b/drivers/video/msm/mdp.c
@@ -52,6 +52,7 @@
 int mdp_rev;
 int mdp_iommu_split_domain;
 u32 mdp_max_clk = 200000000;
+u64 mdp_max_bw = 2000000000;
 
 static struct platform_device *mdp_init_pdev;
 static struct regulator *footswitch, *dsi_pll_vdda, *dsi_pll_vddio;
@@ -1378,34 +1379,49 @@
 #define MAX_VSYNC_GAP		4
 #define DEFAULT_FRAME_RATE	60
 
-static u32 mdp_get_panel_framerate(struct msm_fb_data_type *mfd)
+u32 mdp_get_panel_framerate(struct msm_fb_data_type *mfd)
 {
-	u32 frame_rate = 0, total_pixel;
+	u32 frame_rate = 0, pixel_rate = 0, total_pixel;
 	struct msm_panel_info *panel_info = &mfd->panel_info;
+
+	pixel_rate =
+		(panel_info->type == MIPI_CMD_PANEL ||
+		 panel_info->type == MIPI_VIDEO_PANEL) ?
+		panel_info->mipi.dsi_pclk_rate :
+		panel_info->clk_rate;
+
+	if (!pixel_rate)
+		pr_warn("%s pixel rate is zero\n", __func__);
+
+	total_pixel =
+		(panel_info->lcdc.h_back_porch +
+		 panel_info->lcdc.h_front_porch +
+		 panel_info->lcdc.h_pulse_width +
+		 panel_info->xres) *
+		(panel_info->lcdc.v_back_porch +
+		 panel_info->lcdc.v_front_porch +
+		 panel_info->lcdc.v_pulse_width +
+		 panel_info->yres);
+
+	if (total_pixel)
+		frame_rate = pixel_rate / total_pixel;
+	else
+		pr_warn("%s total pixels are zero\n", __func__);
+
 	if (mfd->dest == DISPLAY_LCD) {
 		if (panel_info->type == MDDI_PANEL && panel_info->mddi.is_type1)
 			frame_rate = panel_info->lcd.refx100 / (100 * 2);
-		else
+		else if (panel_info->type != MIPI_CMD_PANEL)
 			frame_rate = panel_info->lcd.refx100 / 100;
-	} else {
-		if (panel_info->type == MIPI_VIDEO_PANEL) {
-			frame_rate = panel_info->mipi.frame_rate;
-		} else {
-			total_pixel = (panel_info->lcdc.h_back_porch +
-				  panel_info->lcdc.h_front_porch +
-				  panel_info->lcdc.h_pulse_width +
-				  panel_info->xres) *
-				 (panel_info->lcdc.v_back_porch +
-				  panel_info->lcdc.v_front_porch +
-				  panel_info->lcdc.v_pulse_width +
-				  panel_info->yres);
-			if (total_pixel)
-				frame_rate = panel_info->clk_rate /
-					total_pixel;
-		}
 	}
-	if (frame_rate == 0)
+
+	if (frame_rate == 0) {
 		frame_rate = DEFAULT_FRAME_RATE;
+		pr_warn("%s frame rate=%d is default\n", __func__, frame_rate);
+	}
+	pr_debug("%s frame rate=%d total_pixel=%d, pixel_rate=%d\n", __func__,
+		frame_rate, total_pixel, pixel_rate);
+
 	return frame_rate;
 }
 
@@ -1677,7 +1693,9 @@
 			mdp_clk_cnt--;
 			if (mdp_clk_cnt == 0)
 				mdp_clk_disable_unprepare();
-		}
+		} else
+			pr_err("%s: %d: mdp clk off is invalid\n",
+			       __func__, __LINE__);
 	}
 	pr_debug("%s: on=%d cnt=%d\n", __func__, on, mdp_clk_cnt);
 	mutex_unlock(&mdp_suspend_mutex);
@@ -2226,7 +2244,9 @@
 	ret = panel_next_off(pdev);
 	mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_OFF, FALSE);
 	mdp_clk_ctrl(0);
-
+#ifdef CONFIG_MSM_BUS_SCALING
+	mdp_bus_scale_update_request(0, 0);
+#endif
 	if (mdp_rev >= MDP_REV_41 && mfd->panel.type == MIPI_CMD_PANEL)
 		mdp_dsi_cmd_overlay_suspend(mfd);
 	pr_debug("%s:-\n", __func__);
@@ -2334,20 +2354,77 @@
 }
 
 #ifdef CONFIG_MSM_BUS_SCALING
+
+#ifndef MDP_BUS_VECTOR_ENTRY
+#define MDP_BUS_VECTOR_ENTRY(ab_val, ib_val)		\
+	{						\
+		.src = MSM_BUS_MASTER_MDP_PORT0,	\
+		.dst = MSM_BUS_SLAVE_EBI_CH0,		\
+		.ab  = (ab_val),			\
+		.ib  = (ib_val),			\
+	}
+#endif
+/*
+ *    Entry 0 hold 0 request
+ *    Entry 1 and 2 do ping pong request
+ */
+static struct msm_bus_vectors mdp_bus_vectors[] = {
+	MDP_BUS_VECTOR_ENTRY(0, 0),
+	MDP_BUS_VECTOR_ENTRY( 128000000,  160000000),
+	MDP_BUS_VECTOR_ENTRY( 128000000,  160000000),
+};
+
+static struct msm_bus_paths mdp_bus_usecases[ARRAY_SIZE(mdp_bus_vectors)];
+static struct msm_bus_scale_pdata mdp_bus_scale_table = {
+	.usecase = mdp_bus_usecases,
+	.num_usecases = ARRAY_SIZE(mdp_bus_usecases),
+	.name = "mdp",
+};
 static uint32_t mdp_bus_scale_handle;
-int mdp_bus_scale_update_request(uint32_t index)
+static int mdp_bus_scale_register(void)
 {
-	if (!mdp_pdata && (!mdp_pdata->mdp_bus_scale_table
-	     || index > (mdp_pdata->mdp_bus_scale_table->num_usecases - 1))) {
-		printk(KERN_ERR "%s invalid table or index\n", __func__);
-		return -EINVAL;
+	struct msm_bus_scale_pdata *bus_pdata = &mdp_bus_scale_table;
+	int i;
+	for (i = 0; i < bus_pdata->num_usecases; i++) {
+		mdp_bus_usecases[i].num_paths = 1;
+		mdp_bus_usecases[i].vectors = &mdp_bus_vectors[i];
 	}
+	mdp_bus_scale_handle = msm_bus_scale_register_client(bus_pdata);
+	if (!mdp_bus_scale_handle) {
+		pr_err("%s: not able to get bus scale!\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+int mdp_bus_scale_update_request(u64 ab, u64 ib)
+{
+	static int bus_index = 1;
+
 	if (mdp_bus_scale_handle < 1) {
-		pr_debug("%s invalid bus handle\n", __func__);
+		pr_err("%s invalid bus handle\n", __func__);
 		return -EINVAL;
 	}
-	return msm_bus_scale_client_update_request(mdp_bus_scale_handle,
-							index);
+
+	if (!ab)
+		return msm_bus_scale_client_update_request
+			(mdp_bus_scale_handle, 0);
+
+	/* ping pong bus_index between table entry 1 and 2 */
+	bus_index++;
+	bus_index = (bus_index > 2) ? 1 : bus_index;
+
+	mdp_bus_usecases[bus_index].vectors->ab = min(ab, mdp_max_bw);
+	ib = max(ib, ab);
+	mdp_bus_usecases[bus_index].vectors->ib = min(ib, mdp_max_bw);
+
+	pr_debug("%s: handle=%d index=%d ab=%llu ib=%llu\n", __func__,
+		 (u32)mdp_bus_scale_handle, bus_index,
+		 mdp_bus_usecases[bus_index].vectors->ab,
+		 mdp_bus_usecases[bus_index].vectors->ib);
+
+	return msm_bus_scale_client_update_request
+		(mdp_bus_scale_handle, bus_index);
 }
 #endif
 DEFINE_MUTEX(mdp_clk_lock);
@@ -2955,22 +3032,13 @@
 	mdp_clk_ctrl(0);
 
 #ifdef CONFIG_MSM_BUS_SCALING
-	if (!mdp_bus_scale_handle && mdp_pdata &&
-		mdp_pdata->mdp_bus_scale_table) {
-		mdp_bus_scale_handle =
-			msm_bus_scale_register_client(
-					mdp_pdata->mdp_bus_scale_table);
-		if (!mdp_bus_scale_handle) {
-			printk(KERN_ERR "%s not able to get bus scale\n",
-				__func__);
-			return -ENOMEM;
-		}
-	}
+	if (mdp_bus_scale_register())
+		return -ENOMEM;
 
 	/* req bus bandwidth immediately */
-	if (!(mfd->cont_splash_done) && (mfd->panel_info.pdest == DISPLAY_1))
-		mdp_bus_scale_update_request(5);
-
+	if (!(mfd->cont_splash_done))
+		mdp_bus_scale_update_request
+			(MDP_BUS_SCALE_INIT, MDP_BUS_SCALE_INIT);
 #endif
 
 	/* set driver data */
@@ -3014,8 +3082,7 @@
       mdp_probe_err:
 	platform_device_put(msm_fb_dev);
 #ifdef CONFIG_MSM_BUS_SCALING
-	if (mdp_pdata && mdp_pdata->mdp_bus_scale_table &&
-		mdp_bus_scale_handle > 0)
+	if (mdp_bus_scale_handle > 0)
 		msm_bus_scale_unregister_client(mdp_bus_scale_handle);
 #endif
 	return rc;
@@ -3146,8 +3213,7 @@
 	iounmap(msm_mdp_base);
 	pm_runtime_disable(&pdev->dev);
 #ifdef CONFIG_MSM_BUS_SCALING
-	if (mdp_pdata && mdp_pdata->mdp_bus_scale_table &&
-		mdp_bus_scale_handle > 0)
+	if (mdp_bus_scale_handle > 0)
 		msm_bus_scale_unregister_client(mdp_bus_scale_handle);
 #endif
 	return 0;
diff --git a/drivers/video/msm/mdp.h b/drivers/video/msm/mdp.h
index 0bc2532..eab4dbe 100644
--- a/drivers/video/msm/mdp.h
+++ b/drivers/video/msm/mdp.h
@@ -849,12 +849,12 @@
 int mdp_clk_round_rate(u32 rate);
 
 unsigned long mdp_get_core_clk(void);
-unsigned long mdp_perf_level2clk_rate(uint32 perf_level);
 
 #ifdef CONFIG_MSM_BUS_SCALING
-int mdp_bus_scale_update_request(uint32_t index);
+int mdp_bus_scale_update_request(u64 ab, u64 ib);
 #else
-static inline int mdp_bus_scale_update_request(uint32_t index)
+static inline int mdp_bus_scale_update_request(u64 ab,
+					       u64 ib)
 {
 	return 0;
 }
@@ -932,6 +932,8 @@
 	unsigned long srcp1_addr, unsigned long srcp1_size);
 void mdp_update_pm(struct msm_fb_data_type *mfd, ktime_t pre_vsync);
 
+u32 mdp_get_panel_framerate(struct msm_fb_data_type *mfd);
+
 #ifdef CONFIG_FB_MSM_DTV
 void mdp_vid_quant_set(void);
 #else
diff --git a/drivers/video/msm/mdp4.h b/drivers/video/msm/mdp4.h
index 67ef8bf..02cdd71 100644
--- a/drivers/video/msm/mdp4.h
+++ b/drivers/video/msm/mdp4.h
@@ -31,6 +31,12 @@
 extern u32 dbg_force_ov0_blt;
 extern u32 dbg_force_ov1_blt;
 
+extern u64 mdp_max_bw;
+#define MDP4_BW_AB_FACTOR (115)	/* 1.15 */
+#define MDP4_BW_IB_FACTOR (125)	/* 1.25 */
+#define MDP_BUS_SCALE_AB_STEP (0x4000000)
+#define MDP_BUS_SCALE_INIT (0x10000000)
+
 #define MDP4_OVERLAYPROC0_BASE	0x10000
 #define MDP4_OVERLAYPROC1_BASE	0x18000
 #define MDP4_OVERLAYPROC2_BASE	0x88000
@@ -46,13 +52,6 @@
 #define CS_CONTROLLER_0 0x0707ffff
 #define CS_CONTROLLER_1 0x03073f3f
 
-enum {
-	OVERLAY_PERF_LEVEL1 = 1,
-	OVERLAY_PERF_LEVEL2,
-	OVERLAY_PERF_LEVEL3,
-	OVERLAY_PERF_LEVEL4
-};
-
 typedef int (*cmd_fxn_t)(struct platform_device *pdev);
 
 enum {		/* display */
@@ -234,6 +233,12 @@
 #define MDP4_MAX_PLANE		4
 #define VSYNC_PERIOD		16
 
+#ifdef BLT_RGB565
+#define BLT_BPP 2
+#else
+#define BLT_BPP 3
+#endif
+
 struct mdp4_hsic_regs {
 	int32_t params[NUM_HSIC_PARAM];
 	int32_t conv_matrix[3][3];
@@ -358,7 +363,8 @@
 	uint32 blt_dmap_koff;
 	uint32 blt_dmap_done;
 	uint32 req_clk;
-	uint32 req_bw;
+	uint64 bw_ab_quota;
+	uint64 bw_ib_quota;
 	uint32 luma_align_size;
 	struct mdp_overlay_pp_params pp_cfg;
 	struct mdp_overlay req_data;
@@ -968,7 +974,9 @@
 	unsigned long srcp0_addr, unsigned long srcp1_addr,
 	unsigned long srcp2_addr);
 int mdp4_overlay_mdp_pipe_req(struct mdp4_overlay_pipe *pipe,
-				struct msm_fb_data_type *mfd);
+			      struct msm_fb_data_type *mfd);
+int mdp4_calc_blt_mdp_bw(struct msm_fb_data_type *mfd,
+			 struct mdp4_overlay_pipe *pipe);
 int mdp4_overlay_mdp_perf_req(struct msm_fb_data_type *mfd,
 				struct mdp4_overlay_pipe *plist);
 void mdp4_overlay_mdp_perf_upd(struct msm_fb_data_type *mfd, int flag);
diff --git a/drivers/video/msm/mdp4_overlay.c b/drivers/video/msm/mdp4_overlay.c
index f4332dd..43fcb7c 100644
--- a/drivers/video/msm/mdp4_overlay.c
+++ b/drivers/video/msm/mdp4_overlay.c
@@ -107,17 +107,15 @@
 
 struct mdp4_overlay_perf {
 	u32 mdp_clk_rate;
-	u32 use_ov0_blt;
-	u32 use_ov1_blt;
-	u32 mdp_bw;
+	u32 use_ov_blt[MDP4_MIXER_MAX];
+	u64 mdp_ov_ab_bw[MDP4_MIXER_MAX];
+	u64 mdp_ov_ib_bw[MDP4_MIXER_MAX];
+	u32 mdp_ab_bw;
+	u32 mdp_ib_bw;
 };
 
-struct mdp4_overlay_perf perf_request = {
-	.mdp_bw = OVERLAY_PERF_LEVEL4,
-};
-struct mdp4_overlay_perf perf_current = {
-	.mdp_bw = OVERLAY_PERF_LEVEL4,
-};
+struct mdp4_overlay_perf perf_request;
+struct mdp4_overlay_perf perf_current;
 
 static struct ion_client *display_iclient;
 
@@ -2515,12 +2513,10 @@
 
 	if (!pipe) {
 		pr_err("%s: pipe is null!\n", __func__);
-		pipe->req_bw = OVERLAY_PERF_LEVEL4;
 		return ret;
 	}
 	if (!mfd) {
 		pr_err("%s: mfd is null!\n", __func__);
-		pipe->req_bw = OVERLAY_PERF_LEVEL4;
 		return ret;
 	}
 
@@ -2686,18 +2682,13 @@
 	return 0;
 }
 
-#define OVERLAY_VGA_SIZE	0x04B000
-#define OVERLAY_720P_TILE_SIZE  0x0E6000
-#define OVERLAY_WSVGA_SIZE 0x98000 /* 1024x608, align 600 to 32bit */
-
-#define OVERLAY_BUS_SCALE_TABLE_BASE	6
-
-
 static int mdp4_calc_pipe_mdp_bw(struct msm_fb_data_type *mfd,
-			  struct mdp4_overlay_pipe *pipe)
+			 struct mdp4_overlay_pipe *pipe)
 {
-	u32 res;
+	u32 fps;
 	int ret = -EINVAL;
+	u32 quota;
+	u32 shift = 16;
 
 	if (!pipe) {
 		pr_err("%s: pipe is null!\n", __func__);
@@ -2708,28 +2699,79 @@
 		return ret;
 	}
 
-	if (pipe->flags & MDP_DEINTERLACE) {
-		pr_info("%s deinterlace requires max mdp bw.\n",
-			__func__);
-		pipe->req_bw = OVERLAY_PERF_LEVEL1;
-		return 0;
+	fps = mdp_get_panel_framerate(mfd);
+	quota = pipe->src_w * pipe->src_h * fps * pipe->bpp;
+
+	quota >>= shift;
+	/* factor 1.15 for ab */
+	pipe->bw_ab_quota = quota * MDP4_BW_AB_FACTOR / 100;
+	/* factor 1.25 for ib */
+	pipe->bw_ib_quota = quota * MDP4_BW_IB_FACTOR / 100;
+	/* down scaling factor for ib */
+	if ((!pipe->dst_h) && (!pipe->src_h) &&
+	    (pipe->src_h > pipe->dst_h)) {
+		u64 ib = quota;
+		ib *= pipe->src_h;
+		ib /= pipe->dst_h;
+		pipe->bw_ib_quota = max(ib, pipe->bw_ib_quota);
+		pr_debug("%s: src_h=%d dst_h=%d mdp ib %llu, ib_quota=%llu\n",
+			 __func__, pipe->src_h, pipe->dst_h,
+			 ib<<shift, pipe->bw_ib_quota<<shift);
 	}
 
-	if (pipe->pipe_type == OVERLAY_TYPE_BF) {
-		pipe->req_bw = OVERLAY_PERF_LEVEL4;
-		return 0;
+	pipe->bw_ab_quota <<= shift;
+	pipe->bw_ib_quota <<= shift;
+
+	pr_debug("%s: pipe ndx=%d src(h,w)(%d, %d) fps=%d bpp=%d\n",
+		 __func__, pipe->pipe_ndx,  pipe->src_h, pipe->src_w,
+		 fps, pipe->bpp);
+	pr_debug("%s: ab_quota=%llu ib_quota=%llu\n", __func__,
+		 pipe->bw_ab_quota, pipe->bw_ib_quota);
+
+	return 0;
+}
+
+int mdp4_calc_blt_mdp_bw(struct msm_fb_data_type *mfd,
+			 struct mdp4_overlay_pipe *pipe)
+{
+	struct mdp4_overlay_perf *perf_req = &perf_request;
+	u32 fps;
+	int bpp;
+	int ret = -EINVAL;
+	u32 quota;
+	u32 shift = 16;
+	if (!pipe) {
+		pr_err("%s: pipe is null!\n", __func__);
+		return ret;
+	}
+	if (!mfd) {
+		pr_err("%s: mfd is null!\n", __func__);
+		return ret;
 	}
 
-	res = pipe->src_w * pipe->src_h;
+	bpp = BLT_BPP;
+	fps = mdp_get_panel_framerate(mfd);
 
-	if (res <= OVERLAY_WSVGA_SIZE)
-		pipe->req_bw = OVERLAY_PERF_LEVEL4;
-	else if (res <= OVERLAY_VGA_SIZE)
-		pipe->req_bw = OVERLAY_PERF_LEVEL3;
-	else if (res <= OVERLAY_720P_TILE_SIZE)
-		pipe->req_bw = OVERLAY_PERF_LEVEL2;
-	else
-		pipe->req_bw = OVERLAY_PERF_LEVEL1;
+	/* read and write bw*/
+	quota = pipe->dst_w * pipe->dst_h * fps * bpp * 2;
+	quota >>= shift;
+
+	perf_req->mdp_ov_ab_bw[pipe->mixer_num] =
+		quota * MDP4_BW_AB_FACTOR / 100;
+
+	perf_req->mdp_ov_ib_bw[pipe->mixer_num] =
+		quota * MDP4_BW_IB_FACTOR / 100;
+
+	perf_req->mdp_ov_ab_bw[pipe->mixer_num] <<= shift;
+	perf_req->mdp_ov_ib_bw[pipe->mixer_num] <<= shift;
+
+	pr_debug("%s: pipe ndx=%d dst(h,w)(%d, %d) fps=%d bpp=%d\n",
+		 __func__, pipe->pipe_ndx, pipe->dst_h, pipe->dst_w,
+		 fps, bpp);
+	pr_debug("%s: overlay=%d ab_bw=%llu ib_bw=%llu\n", __func__,
+		 pipe->mixer_num,
+		 perf_req->mdp_ov_ab_bw[pipe->mixer_num],
+		 perf_req->mdp_ov_ib_bw[pipe->mixer_num]);
 
 	return 0;
 }
@@ -2738,12 +2780,12 @@
 			      struct mdp4_overlay_pipe *plist)
 {
 	u32 worst_mdp_clk = 0;
-	u32 worst_mdp_bw = OVERLAY_PERF_LEVEL4;
 	int i;
 	struct mdp4_overlay_perf *perf_req = &perf_request;
 	struct mdp4_overlay_pipe *pipe = plist;
 	u32 cnt = 0;
 	int ret = -EINVAL;
+	u64 ab_quota_total = 0, ib_quota_total = 0;
 
 	if (!mfd) {
 		pr_err("%s: mfd is null!\n", __func__);
@@ -2755,8 +2797,8 @@
 		return ret;
 	}
 
-	perf_req->use_ov0_blt = 0;
-	perf_req->use_ov1_blt = 0;
+	for (i = 0; i < MDP4_MIXER_MAX; i++)
+		perf_req->use_ov_blt[i] = 0;
 
 	for (i = 0; i < OVERLAY_PIPE_MAX; i++, pipe++) {
 
@@ -2768,25 +2810,17 @@
 		cnt++;
 		if (worst_mdp_clk < pipe->req_clk)
 			worst_mdp_clk = pipe->req_clk;
-		if (pipe->req_clk > mdp_max_clk) {
-			if (pipe->mixer_num == MDP4_MIXER0)
-				perf_req->use_ov0_blt = 1;
-			if (pipe->mixer_num == MDP4_MIXER1)
-				perf_req->use_ov1_blt = 1;
-		}
 
-		if (!pipe->req_bw) {
-			pr_err("%s mdp pipe bw request should not be zero!\n",
-			       __func__);
-			pr_debug("%s %d pid %d num %d idx %d mix %d bw %d\n",
-				 __func__, __LINE__, current->pid,
-				 pipe->pipe_num, pipe->pipe_ndx,
-				 pipe->mixer_num, pipe->req_bw);
-			pipe->req_bw = OVERLAY_PERF_LEVEL4;
-		}
+		if (pipe->req_clk > mdp_max_clk)
+			perf_req->use_ov_blt[pipe->mixer_num] = 1;
 
-		if (pipe->req_bw < worst_mdp_bw)
-			worst_mdp_bw = pipe->req_bw;
+		if (pipe->mixer_num == MDP4_MIXER2)
+			perf_req->use_ov_blt[MDP4_MIXER2] = 1;
+
+		if (pipe->pipe_type != OVERLAY_TYPE_BF) {
+			ab_quota_total += pipe->bw_ab_quota;
+			ib_quota_total += pipe->bw_ib_quota;
+		}
 
 		if (mfd->mdp_rev == MDP_REV_41) {
 			/*
@@ -2795,32 +2829,45 @@
 			 */
 			if (ctrl->panel_mode & MDP4_PANEL_DSI_CMD) {
 				if (pipe->dst_x != 0)
-					perf_req->use_ov0_blt = 1;
+					perf_req->use_ov_blt[MDP4_MIXER0] = 1;
 			}
 			if ((mfd->panel_info.xres > 1280) &&
 			    (mfd->panel_info.type != DTV_PANEL)) {
-				perf_req->use_ov0_blt = 1;
+				perf_req->use_ov_blt[MDP4_MIXER0] = 1;
 			}
 		}
 	}
 
-	perf_req->mdp_clk_rate = worst_mdp_clk;
-	if (perf_req->mdp_clk_rate > mdp_max_clk)
-		perf_req->mdp_clk_rate = mdp_max_clk;
-
+	perf_req->mdp_clk_rate = min(worst_mdp_clk, mdp_max_clk);
 	perf_req->mdp_clk_rate = mdp_clk_round_rate(perf_req->mdp_clk_rate);
 
-	perf_req->mdp_bw = worst_mdp_bw;
+	for (i = 0; i < MDP4_MIXER_MAX; i++) {
+		if (perf_req->use_ov_blt[i]) {
+			ab_quota_total += perf_req->mdp_ov_ab_bw[i];
+			ib_quota_total += perf_req->mdp_ov_ib_bw[i];
+		}
+	}
 
-	if (cnt >= 3)
-		perf_req->mdp_bw = OVERLAY_PERF_LEVEL1;
+	perf_req->mdp_ab_bw = roundup(ab_quota_total, MDP_BUS_SCALE_AB_STEP);
+	perf_req->mdp_ib_bw = roundup(ib_quota_total, MDP_BUS_SCALE_AB_STEP);
 
-	pr_debug("%s %d pid %d cnt %d clk %d ov0_blt %d, ov1_blt %d bw %d\n",
+	pr_debug("%s %d: ab_quota_total=(%llu, %d) ib_quota_total=(%llu, %d)\n",
+		 __func__, __LINE__,
+		 ab_quota_total, perf_req->mdp_ab_bw,
+		 ib_quota_total, perf_req->mdp_ib_bw);
+
+	if (ab_quota_total > mdp_max_bw)
+		pr_warn("%s: req ab bw=%llu is larger than max bw=%llu",
+			__func__, ab_quota_total, mdp_max_bw);
+	if (ib_quota_total > mdp_max_bw)
+		pr_warn("%s: req ib bw=%llu is larger than max bw=%llu",
+			__func__, ib_quota_total, mdp_max_bw);
+
+	pr_debug("%s %d: pid %d cnt %d clk %d ov0_blt %d, ov1_blt %d\n",
 		 __func__, __LINE__, current->pid, cnt,
 		 perf_req->mdp_clk_rate,
-		 perf_req->use_ov0_blt,
-		 perf_req->use_ov1_blt,
-		 perf_req->mdp_bw);
+		 perf_req->use_ov_blt[0],
+		 perf_req->use_ov_blt[1]);
 
 	return 0;
 }
@@ -2856,7 +2903,7 @@
 		 flag);
 
 	if (!mdp4_extn_disp)
-		perf_cur->use_ov1_blt = 0;
+		perf_cur->use_ov_blt[1] = 0;
 
 	if (flag) {
 		if (perf_req->mdp_clk_rate > perf_cur->mdp_clk_rate) {
@@ -2869,19 +2916,26 @@
 			perf_cur->mdp_clk_rate =
 				perf_req->mdp_clk_rate;
 		}
-		if (perf_req->mdp_bw < perf_cur->mdp_bw) {
+		if ((perf_req->mdp_ab_bw > perf_cur->mdp_ab_bw) ||
+		    (perf_req->mdp_ib_bw > perf_cur->mdp_ib_bw)) {
 			mdp_bus_scale_update_request
-				(OVERLAY_BUS_SCALE_TABLE_BASE -
-				 perf_req->mdp_bw);
-			pr_info("%s mdp bw is changed [%d] from %d to %d\n",
+				(perf_req->mdp_ab_bw, perf_req->mdp_ib_bw);
+			pr_debug("%s mdp ab_bw is changed [%d] from %d to %d\n",
 				__func__,
 				flag,
-				perf_cur->mdp_bw,
-				perf_req->mdp_bw);
-			perf_cur->mdp_bw = perf_req->mdp_bw;
+				perf_cur->mdp_ab_bw,
+				perf_req->mdp_ab_bw);
+			pr_debug("%s mdp ib_bw is changed [%d] from %d to %d\n",
+				__func__,
+				flag,
+				perf_cur->mdp_ib_bw,
+				perf_req->mdp_ib_bw);
+			perf_cur->mdp_ab_bw = perf_req->mdp_ab_bw;
+			perf_cur->mdp_ib_bw = perf_req->mdp_ib_bw;
 		}
+
 		if ((mfd->panel_info.pdest == DISPLAY_1 &&
-		     perf_req->use_ov0_blt && !perf_cur->use_ov0_blt) ||
+		     perf_req->use_ov_blt[0] && !perf_cur->use_ov_blt[0]) ||
 		    dbg_force_ov0_blt) {
 			if (mfd->panel_info.type == LCDC_PANEL ||
 			    mfd->panel_info.type == LVDS_PANEL)
@@ -2895,20 +2949,20 @@
 			pr_debug("%s mixer0 start blt [%d] from %d to %d.\n",
 				__func__,
 				flag,
-				perf_cur->use_ov0_blt,
-				perf_req->use_ov0_blt);
-			perf_cur->use_ov0_blt = perf_req->use_ov0_blt;
+				perf_cur->use_ov_blt[0],
+				perf_req->use_ov_blt[0]);
+			perf_cur->use_ov_blt[0] = perf_req->use_ov_blt[0];
 		}
 		if ((mfd->panel_info.pdest == DISPLAY_2 &&
-		     perf_req->use_ov1_blt && !perf_cur->use_ov1_blt) ||
+		     perf_req->use_ov_blt[1] && !perf_cur->use_ov_blt[1]) ||
 		    dbg_force_ov1_blt) {
 			mdp4_dtv_overlay_blt_start(mfd);
 			pr_debug("%s mixer1 start blt [%d] from %d to %d.\n",
 				__func__,
 				flag,
-				perf_cur->use_ov1_blt,
-				perf_req->use_ov1_blt);
-			perf_cur->use_ov1_blt = perf_req->use_ov1_blt;
+				perf_cur->use_ov_blt[1],
+				perf_req->use_ov_blt[1]);
+			perf_cur->use_ov_blt[1] = perf_req->use_ov_blt[1];
 		}
 	} else {
 		if (perf_req->mdp_clk_rate < perf_cur->mdp_clk_rate) {
@@ -2921,19 +2975,25 @@
 			perf_cur->mdp_clk_rate =
 				perf_req->mdp_clk_rate;
 		}
-		if (perf_req->mdp_bw > perf_cur->mdp_bw) {
-			pr_info("%s mdp bw is changed [%d] from %d to %d\n",
+		if (perf_req->mdp_ab_bw < perf_cur->mdp_ab_bw ||
+		    perf_req->mdp_ib_bw < perf_cur->mdp_ib_bw) {
+			mdp_bus_scale_update_request
+				(perf_req->mdp_ab_bw, perf_req->mdp_ib_bw);
+			pr_debug("%s mdp ab bw is changed [%d] from %d to %d\n",
 				__func__,
 				flag,
-				perf_cur->mdp_bw,
-				perf_req->mdp_bw);
-			mdp_bus_scale_update_request
-				(OVERLAY_BUS_SCALE_TABLE_BASE -
-				 perf_req->mdp_bw);
-			perf_cur->mdp_bw = perf_req->mdp_bw;
+				perf_cur->mdp_ab_bw,
+				perf_req->mdp_ab_bw);
+			pr_debug("%s mdp ib bw is changed [%d] from %d to %d\n",
+				__func__,
+				flag,
+				perf_cur->mdp_ib_bw,
+				perf_req->mdp_ib_bw);
+			perf_cur->mdp_ab_bw = perf_req->mdp_ab_bw;
+			perf_cur->mdp_ib_bw = perf_req->mdp_ib_bw;
 		}
 		if ((mfd->panel_info.pdest == DISPLAY_1 &&
-		     !perf_req->use_ov0_blt && perf_cur->use_ov0_blt) ||
+		     !perf_req->use_ov_blt[0] && perf_cur->use_ov_blt[0]) ||
 		    dbg_force_ov0_blt) {
 			if (mfd->panel_info.type == LCDC_PANEL ||
 			    mfd->panel_info.type == LVDS_PANEL)
@@ -2947,20 +3007,20 @@
 			pr_debug("%s mixer0 stop blt [%d] from %d to %d.\n",
 				__func__,
 				flag,
-				perf_cur->use_ov0_blt,
-				perf_req->use_ov0_blt);
-			perf_cur->use_ov0_blt = perf_req->use_ov0_blt;
+				perf_cur->use_ov_blt[0],
+				perf_req->use_ov_blt[0]);
+			perf_cur->use_ov_blt[0] = perf_req->use_ov_blt[0];
 		}
 		if ((mfd->panel_info.pdest == DISPLAY_2 &&
-		     !perf_req->use_ov1_blt && perf_cur->use_ov1_blt) ||
+		     !perf_req->use_ov_blt[1] && perf_cur->use_ov_blt[1]) ||
 		    dbg_force_ov1_blt) {
 			mdp4_dtv_overlay_blt_stop(mfd);
 			pr_debug("%s mixer1 stop blt [%d] from %d to %d.\n",
 				__func__,
 				flag,
-				perf_cur->use_ov1_blt,
-				perf_req->use_ov1_blt);
-			perf_cur->use_ov1_blt = perf_req->use_ov1_blt;
+				perf_cur->use_ov_blt[1],
+				perf_req->use_ov_blt[1]);
+			perf_cur->use_ov_blt[1] = perf_req->use_ov_blt[1];
 		}
 	}
 	return;
diff --git a/drivers/video/msm/mdp4_overlay_dsi_video.c b/drivers/video/msm/mdp4_overlay_dsi_video.c
index 450f1de..a4d2b77 100644
--- a/drivers/video/msm/mdp4_overlay_dsi_video.c
+++ b/drivers/video/msm/mdp4_overlay_dsi_video.c
@@ -568,6 +568,7 @@
 	pipe->dst_w = fbi->var.xres;
 
 	mdp4_overlay_mdp_pipe_req(pipe, mfd);
+	mdp4_calc_blt_mdp_bw(mfd, pipe);
 
 	atomic_set(&vctrl->suspend, 0);
 
diff --git a/drivers/video/msm/mdp4_overlay_dtv.c b/drivers/video/msm/mdp4_overlay_dtv.c
index 7de2e2a..85fb305 100644
--- a/drivers/video/msm/mdp4_overlay_dtv.c
+++ b/drivers/video/msm/mdp4_overlay_dtv.c
@@ -869,6 +869,7 @@
 	pipe->srcp0_ystride = fbi->fix.line_length;
 
 	mdp4_overlay_mdp_pipe_req(pipe, mfd);
+	mdp4_calc_blt_mdp_bw(mfd, pipe);
 
 	ret = mdp4_overlay_format2pipe(pipe);
 	if (ret < 0)
diff --git a/drivers/video/msm/mdp4_overlay_lcdc.c b/drivers/video/msm/mdp4_overlay_lcdc.c
index a7058ce..4a46d72 100644
--- a/drivers/video/msm/mdp4_overlay_lcdc.c
+++ b/drivers/video/msm/mdp4_overlay_lcdc.c
@@ -546,6 +546,7 @@
 	pipe->bpp = bpp;
 
 	mdp4_overlay_mdp_pipe_req(pipe, mfd);
+	mdp4_calc_blt_mdp_bw(mfd, pipe);
 
 	atomic_set(&vctrl->suspend, 0);
 
diff --git a/drivers/video/msm/mdp4_overlay_writeback.c b/drivers/video/msm/mdp4_overlay_writeback.c
index aa50d94..bd20e82 100644
--- a/drivers/video/msm/mdp4_overlay_writeback.c
+++ b/drivers/video/msm/mdp4_overlay_writeback.c
@@ -61,6 +61,7 @@
 	struct msm_fb_data_type *mfd;
 	struct mdp4_overlay_pipe *base_pipe;
 	struct vsync_update vlist[2];
+	struct work_struct clk_work;
 } vsync_ctrl_db[MAX_CONTROLLER];
 
 static void vsync_irq_enable(int intr, int term)
@@ -260,6 +261,7 @@
 	pipe->dst_x = 0;
 
 	mdp4_overlay_mdp_pipe_req(pipe, mfd);
+	mdp4_calc_blt_mdp_bw(mfd, pipe);
 
 	if (mfd->display_iova)
 		pipe->srcp0_addr = mfd->display_iova + buf_offset;
@@ -375,6 +377,8 @@
 		}
 	}
 
+	mdp_clk_ctrl(1);
+
 	mdp4_mixer_stage_commit(mixer);
 
 	pipe = vctrl->base_pipe;
@@ -399,6 +403,13 @@
 	return cnt;
 }
 
+static void clk_ctrl_work(struct work_struct *work)
+{
+	struct vsycn_ctrl *vctrl =
+		container_of(work, typeof(*vctrl), clk_work);
+	mdp_clk_ctrl(0);
+}
+
 void mdp4_wfd_init(int cndx)
 {
 	struct vsycn_ctrl *vctrl;
@@ -417,6 +428,7 @@
 	mutex_init(&vctrl->update_lock);
 	init_completion(&vctrl->ov_comp);
 	spin_lock_init(&vctrl->spin_lock);
+	INIT_WORK(&vctrl->clk_work, clk_ctrl_work);
 }
 
 static void mdp4_wfd_wait4ov(int cndx)
@@ -450,7 +462,7 @@
 	vsync_irq_disable(INTR_OVERLAY2_DONE, MDP_OVERLAY2_TERM);
 	vctrl->ov_done++;
 	complete(&vctrl->ov_comp);
-
+	schedule_work(&vctrl->clk_work);
 	pr_debug("%s ovdone interrupt\n", __func__);
 	spin_unlock(&vctrl->spin_lock);
 }
@@ -475,16 +487,11 @@
 
 	mdp4_overlay_mdp_perf_upd(mfd, 1);
 
-	mdp_clk_ctrl(1);
-
 	mdp4_wfd_pipe_commit(mfd, 0, 1);
 
 	mdp4_overlay_mdp_perf_upd(mfd, 0);
 
-	mdp_clk_ctrl(0);
-
 	mutex_unlock(&mfd->dma->ov_mutex);
-
 }
 
 static int mdp4_overlay_writeback_register_buffer(
diff --git a/drivers/video/msm/mdp4_util.c b/drivers/video/msm/mdp4_util.c
index b6c2634..01ec10e 100644
--- a/drivers/video/msm/mdp4_util.c
+++ b/drivers/video/msm/mdp4_util.c
@@ -392,7 +392,8 @@
 	/* MDP cmd block enable */
 	mdp_pipe_ctrl(MDP_CMD_BLOCK, MDP_BLOCK_POWER_ON, FALSE);
 
-	mdp_bus_scale_update_request(5);
+	mdp_bus_scale_update_request
+		(MDP_BUS_SCALE_INIT, MDP_BUS_SCALE_INIT);
 
 #ifdef MDP4_ERROR
 	/*
diff --git a/drivers/video/msm/mdss/Makefile b/drivers/video/msm/mdss/Makefile
index a0d707e..4deaa8c 100644
--- a/drivers/video/msm/mdss/Makefile
+++ b/drivers/video/msm/mdss/Makefile
@@ -20,5 +20,6 @@
 obj-$(CONFIG_FB_MSM_MDSS_HDMI_PANEL) += mdss_hdmi_util.o
 obj-$(CONFIG_FB_MSM_MDSS_HDMI_PANEL) += mdss_hdmi_edid.o
 obj-$(CONFIG_FB_MSM_MDSS_HDMI_MHL_8334) += mhl_sii8334.o
+obj-$(CONFIG_FB_MSM_MDSS_HDMI_PANEL) += mdss_hdmi_hdcp.o
 
 obj-$(CONFIG_FB_MSM_MDSS_WRITEBACK) += mdss_wb.o
diff --git a/drivers/video/msm/mdss/mdss_fb.c b/drivers/video/msm/mdss/mdss_fb.c
index 581ec17..2e8a654 100644
--- a/drivers/video/msm/mdss/mdss_fb.c
+++ b/drivers/video/msm/mdss/mdss_fb.c
@@ -847,11 +847,11 @@
 	var->xres_virtual = panel_info->xres;
 	var->yres_virtual = panel_info->yres * mfd->fb_page;
 	var->bits_per_pixel = bpp * 8;	/* FrameBuffer color depth */
-	var->upper_margin = panel_info->lcdc.v_front_porch;
-	var->lower_margin = panel_info->lcdc.v_back_porch;
+	var->upper_margin = panel_info->lcdc.v_back_porch;
+	var->lower_margin = panel_info->lcdc.v_front_porch;
 	var->vsync_len = panel_info->lcdc.v_pulse_width;
-	var->left_margin = panel_info->lcdc.h_front_porch;
-	var->right_margin = panel_info->lcdc.h_back_porch;
+	var->left_margin = panel_info->lcdc.h_back_porch;
+	var->right_margin = panel_info->lcdc.h_front_porch;
 	var->hsync_len = panel_info->lcdc.h_pulse_width;
 	var->pixclock = panel_info->clk_rate / 1000;
 
@@ -1006,11 +1006,11 @@
 {
 	pinfo->xres = var->xres;
 	pinfo->yres = var->yres;
-	pinfo->lcdc.v_front_porch = var->upper_margin;
-	pinfo->lcdc.v_back_porch = var->lower_margin;
+	pinfo->lcdc.v_front_porch = var->lower_margin;
+	pinfo->lcdc.v_back_porch = var->upper_margin;
 	pinfo->lcdc.v_pulse_width = var->vsync_len;
-	pinfo->lcdc.h_front_porch = var->left_margin;
-	pinfo->lcdc.h_back_porch = var->right_margin;
+	pinfo->lcdc.h_front_porch = var->right_margin;
+	pinfo->lcdc.h_back_porch = var->left_margin;
 	pinfo->lcdc.h_pulse_width = var->hsync_len;
 	pinfo->clk_rate = var->pixclock;
 	/* todo: find how to pass CEA vic through framebuffer APIs */
diff --git a/drivers/video/msm/mdss/mdss_hdmi_edid.c b/drivers/video/msm/mdss/mdss_hdmi_edid.c
index 8db38d6..6c76348 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_edid.c
+++ b/drivers/video/msm/mdss/mdss_hdmi_edid.c
@@ -16,9 +16,17 @@
 #include "mdss_hdmi_edid.h"
 
 #define DBC_START_OFFSET 4
-#define HDMI_VSDB_3D_DATA_OFFSET(vsd) \
+#define HDMI_VSDB_3D_EVF_DATA_OFFSET(vsd) \
 	(!((vsd)[8] & BIT(7)) ? 9 : (!((vsd)[8] & BIT(6)) ? 11 : 13))
 
+/*
+ * As per the CEA-861E spec, there can be a total of 10 short audio
+ * descriptors with each SAD being 3 bytes long.
+ * Thus, the maximum length of the audio data block would be 30 bytes
+ */
+#define MAX_AUDIO_DATA_BLOCK_SIZE	30
+#define MAX_SPKR_ALLOC_DATA_BLOCK_SIZE	3
+
 struct hdmi_edid_sink_data {
 	u32 disp_mode_list[HDMI_VFRMT_MAX];
 	u32 disp_3d_mode_list[HDMI_VFRMT_MAX];
@@ -35,11 +43,13 @@
 	u16 physical_address;
 	u32 video_resolution; /* selected by user */
 	u32 sink_mode; /* HDMI or DVI */
-	u8 speaker_allocation_block;
-	u8 audio_data_block_cnt;
 	u16 audio_latency;
 	u16 video_latency;
 	u32 present_3d;
+	u8 audio_data_block[MAX_AUDIO_DATA_BLOCK_SIZE];
+	int adb_size;
+	u8 spkr_alloc_data_block[MAX_SPKR_ALLOC_DATA_BLOCK_SIZE];
+	int sadb_size;
 
 	struct hdmi_edid_sink_data sink_data;
 	struct hdmi_edid_init_data init_data;
@@ -180,6 +190,10 @@
 	{HDMI_VFRMT_1920x1080i120_16_9, 1920, 1080, true,  2200, 280, 1125,
 	 22, 67500, 120000, 148500, 120000, false},
 
+	/* All 2560 H Active */
+	{HDMI_VFRMT_2560x1600p60_16_9, 2560, 1600, false, 2720, 160, 1646,
+	 46, 98700, 60000, 268500, 60000, false},
+
 	/* All 2880 H Active */
 	{HDMI_VFRMT_2880x576i50_4_3, 2880, 576, true,  3456, 576, 625, 24,
 	 15625, 50000, 54000, 50000, true},
@@ -437,7 +451,7 @@
 		u8 block_len = in_buf[offset] & 0x1F;
 		if ((in_buf[offset] >> 5) == type) {
 			*len = block_len;
-			DEV_DBG("%s: EDID: block=%d found @ %d w/ length=%d\n",
+			DEV_DBG("%s: EDID: block=%d found @ 0x%x w/ len=%d\n",
 				__func__, type, offset, block_len);
 
 			return in_buf + offset;
@@ -533,8 +547,8 @@
 		return;
 	}
 
-	offset = HDMI_VSDB_3D_DATA_OFFSET(vsd);
-	DEV_DBG("%s: EDID: 3D present @ %d = %02x\n", __func__,
+	offset = HDMI_VSDB_3D_EVF_DATA_OFFSET(vsd);
+	DEV_DBG("%s: EDID: 3D present @ 0x%x = %02x\n", __func__,
 		offset, vsd[offset]);
 
 	if (vsd[offset] >> 7) { /* 3D format indication present */
@@ -547,26 +561,28 @@
 static void hdmi_edid_extract_audio_data_blocks(
 	struct hdmi_edid_ctrl *edid_ctrl, const u8 *in_buf)
 {
-	u8 len;
-	const u8 *sad = NULL;
+	u8 len, cnt = 0;
+	const u8 *adb = NULL;
 
 	if (!edid_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
 		return;
 	}
 
-	sad = hdmi_edid_find_block(in_buf, DBC_START_OFFSET, 1, &len);
-	if (sad == NULL)
+	adb = hdmi_edid_find_block(in_buf, DBC_START_OFFSET, 1, &len);
+	if ((adb == NULL) || (len > MAX_AUDIO_DATA_BLOCK_SIZE))
 		return;
 
-	edid_ctrl->audio_data_block_cnt = 0;
-	while (len >= 3 && edid_ctrl->audio_data_block_cnt < 16) {
-		DEV_DBG("%s: ch=%d fmt=%d sampling=0x%02x bitdepth=0x%02x\n",
-			__func__, (sad[1]&0x7)+1, sad[1]>>3, sad[2], sad[3]);
+	memcpy(edid_ctrl->audio_data_block, adb + 1, len);
+	edid_ctrl->adb_size = len;
 
-		++edid_ctrl->audio_data_block_cnt;
+	while (len >= 3 && cnt < 16) {
+		DEV_DBG("%s: ch=%d fmt=%d sampling=0x%02x bitdepth=0x%02x\n",
+			__func__, (adb[1]&0x7)+1, adb[1]>>3, adb[2], adb[3]);
+
+		cnt++;
 		len -= 3;
-		sad += 3;
+		adb += 3;
 	}
 } /* hdmi_edid_extract_audio_data_blocks */
 
@@ -574,27 +590,29 @@
 	struct hdmi_edid_ctrl *edid_ctrl, const u8 *in_buf)
 {
 	u8 len;
-	const u8 *sad = NULL;
+	const u8 *sadb = NULL;
 
 	if (!edid_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
 		return;
 	}
 
-	sad = hdmi_edid_find_block(in_buf, DBC_START_OFFSET, 4, &len);
-	if (sad == NULL)
+	sadb = hdmi_edid_find_block(in_buf, DBC_START_OFFSET, 4, &len);
+	if ((sadb == NULL) || (len != MAX_SPKR_ALLOC_DATA_BLOCK_SIZE))
 		return;
 
-	edid_ctrl->speaker_allocation_block = sad[1];
+	memcpy(edid_ctrl->spkr_alloc_data_block, sadb + 1, len);
+	edid_ctrl->sadb_size = len;
+
 	DEV_DBG("%s: EDID: speaker alloc data SP byte = %08x %s%s%s%s%s%s%s\n",
-		__func__, sad[1],
-		(sad[1] & BIT(0)) ? "FL/FR," : "",
-		(sad[1] & BIT(1)) ? "LFE," : "",
-		(sad[1] & BIT(2)) ? "FC," : "",
-		(sad[1] & BIT(3)) ? "RL/RR," : "",
-		(sad[1] & BIT(4)) ? "RC," : "",
-		(sad[1] & BIT(5)) ? "FLC/FRC," : "",
-		(sad[1] & BIT(6)) ? "RLC/RRC," : "");
+		__func__, sadb[1],
+		(sadb[1] & BIT(0)) ? "FL/FR," : "",
+		(sadb[1] & BIT(1)) ? "LFE," : "",
+		(sadb[1] & BIT(2)) ? "FC," : "",
+		(sadb[1] & BIT(3)) ? "RL/RR," : "",
+		(sadb[1] & BIT(4)) ? "RC," : "",
+		(sadb[1] & BIT(5)) ? "FLC/FRC," : "",
+		(sadb[1] & BIT(6)) ? "RLC/RRC," : "");
 } /* hdmi_edid_extract_speaker_allocation_data */
 
 static void hdmi_edid_extract_latency_fields(struct hdmi_edid_ctrl *edid_ctrl,
@@ -835,7 +853,7 @@
 				3, &len) : NULL;
 	int i;
 
-	offset = HDMI_VSDB_3D_DATA_OFFSET(vsd);
+	offset = HDMI_VSDB_3D_EVF_DATA_OFFSET(vsd);
 	present_multi_3d = (vsd[offset] & 0x60) >> 5;
 
 	offset += 1;
@@ -897,17 +915,16 @@
 
 	i = 0;
 	while (hdmi_3d_len > 0) {
-		DEV_DBG("%s: EDID[3D]: 3D_Structure_%d @ %d: %02x\n", __func__,
-			i + 1, offset, vsd[offset]);
+		DEV_DBG("%s: EDID: 3D_Structure_%d @ 0x%x: %02x\n",
+			__func__, i + 1, offset, vsd[offset]);
 
 		if ((vsd[offset] >> 4) >=
 			sink_data->disp_multi_3d_mode_list_cnt) {
 			if ((vsd[offset] & 0x0F) >= 8) {
 				offset += 1;
 				hdmi_3d_len -= 1;
-				DEV_DBG("%s:EDID[3D]:3D_Detail_%d @ %d: %02x\n",
-					__func__, i + 1, offset,
-					vsd[offset]);
+				DEV_DBG("%s:EDID:3D_Detail_%d @ 0x%x: %02x\n",
+					__func__, i + 1, offset, vsd[offset]);
 			}
 			i += 1;
 			offset += 1;
@@ -941,7 +958,7 @@
 		if ((vsd[offset] & 0x0F) >= 8) {
 			offset += 1;
 			hdmi_3d_len -= 1;
-			DEV_DBG("%s: EDID[3D]: 3D_Detail_%d @ %d: %02x\n",
+			DEV_DBG("%s: EDID[3D]: 3D_Detail_%d @ 0x%x: %02x\n",
 				__func__, i + 1, offset,
 				vsd[offset]);
 		}
@@ -951,6 +968,49 @@
 	}
 } /* hdmi_edid_get_display_vsd_3d_mode */
 
+static void hdmi_edid_get_extended_video_formats(
+	struct hdmi_edid_ctrl *edid_ctrl, const u8 *in_buf)
+{
+	u8 db_len, offset, i;
+	u8 hdmi_vic_len;
+	u32 video_format;
+	const u8 *vsd = NULL;
+
+	if (!edid_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	vsd = hdmi_edid_find_block(in_buf, DBC_START_OFFSET, 3, &db_len);
+
+	if (vsd == NULL || db_len < 9) {
+		DEV_DBG("%s: blk-id 3 not found or not long enough\n",
+			__func__);
+		return;
+	}
+
+	/* check if HDMI_Video_present flag is set or not */
+	if (!(vsd[8] & BIT(5))) {
+		DEV_DBG("%s: extended vfmts are not supported by the sink.\n",
+			__func__);
+		return;
+	}
+
+	offset = HDMI_VSDB_3D_EVF_DATA_OFFSET(vsd);
+
+	hdmi_vic_len = vsd[offset + 1] >> 5;
+	if (hdmi_vic_len) {
+		DEV_DBG("%s: EDID: EVFRMT @ 0x%x of block 3, len = %02x\n",
+			__func__, offset, hdmi_vic_len);
+
+		for (i = 0; i < hdmi_vic_len; i++) {
+			video_format = HDMI_VFRMT_END + vsd[offset + 2 + i];
+			hdmi_edid_add_sink_video_format(&edid_ctrl->sink_data,
+				video_format);
+		}
+	}
+} /* hdmi_edid_get_extended_video_formats */
+
 static void hdmi_edid_get_display_mode(struct hdmi_edid_ctrl *edid_ctrl,
 	const u8 *data_buf, u32 num_of_cea_blocks)
 {
@@ -1113,6 +1173,8 @@
 		}
 	}
 
+	hdmi_edid_get_extended_video_formats(edid_ctrl, data_buf+0x80);
+
 	/* mandaroty 3d format */
 	if (edid_ctrl->present_3d) {
 		if (has60hz_mode) {
@@ -1177,6 +1239,12 @@
 	edid_ctrl->present_3d = 0;
 	memset(&edid_ctrl->sink_data, 0, sizeof(edid_ctrl->sink_data));
 	memset(edid_buf, 0, sizeof(edid_buf));
+	memset(edid_ctrl->audio_data_block, 0,
+		sizeof(edid_ctrl->audio_data_block));
+	memset(edid_ctrl->spkr_alloc_data_block, 0,
+		sizeof(edid_ctrl->spkr_alloc_data_block));
+	edid_ctrl->adb_size = 0;
+	edid_ctrl->sadb_size = 0;
 
 	status = hdmi_edid_read_block(edid_ctrl, 0, edid_buf);
 	if (status || !hdmi_edid_check_header(edid_buf)) {
@@ -1353,6 +1421,24 @@
 	return edid_ctrl->sink_mode;
 } /* hdmi_edid_get_sink_mode */
 
+int hdmi_edid_get_audio_blk(void *input, struct msm_hdmi_audio_edid_blk *blk)
+{
+	struct hdmi_edid_ctrl *edid_ctrl = (struct hdmi_edid_ctrl *)input;
+
+	if (!edid_ctrl || !blk) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -EINVAL;
+	}
+
+	blk->audio_data_blk = edid_ctrl->audio_data_block;
+	blk->audio_data_blk_size = edid_ctrl->adb_size;
+
+	blk->spk_alloc_data_blk = edid_ctrl->spkr_alloc_data_block;
+	blk->spk_alloc_data_blk_size = edid_ctrl->sadb_size;
+
+	return 0;
+} /* hdmi_edid_get_audio_blk */
+
 void hdmi_edid_set_video_resolution(void *input, u32 resolution)
 {
 	struct hdmi_edid_ctrl *edid_ctrl = (struct hdmi_edid_ctrl *)input;
diff --git a/drivers/video/msm/mdss/mdss_hdmi_edid.h b/drivers/video/msm/mdss/mdss_hdmi_edid.h
index 5c51e7e..bb56a24 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_edid.h
+++ b/drivers/video/msm/mdss/mdss_hdmi_edid.h
@@ -13,6 +13,7 @@
 #ifndef __HDMI_EDID_H__
 #define __HDMI_EDID_H__
 
+#include <mach/msm_hdmi_audio_codec.h>
 #include "mdss_hdmi_util.h"
 
 struct hdmi_edid_init_data {
@@ -26,6 +27,8 @@
 int hdmi_edid_read(void *edid_ctrl);
 u8 hdmi_edid_get_sink_scaninfo(void *edid_ctrl, u32 resolution);
 u32 hdmi_edid_get_sink_mode(void *edid_ctrl);
+int hdmi_edid_get_audio_blk(void *edid_ctrl,
+	struct msm_hdmi_audio_edid_blk *blk);
 void hdmi_edid_set_video_resolution(void *edid_ctrl, u32 resolution);
 void hdmi_edid_deinit(void *edid_ctrl);
 void *hdmi_edid_init(struct hdmi_edid_init_data *init_data);
diff --git a/drivers/video/msm/mdss/mdss_hdmi_hdcp.c b/drivers/video/msm/mdss/mdss_hdmi_hdcp.c
new file mode 100644
index 0000000..2e20787
--- /dev/null
+++ b/drivers/video/msm/mdss/mdss_hdmi_hdcp.c
@@ -0,0 +1,1153 @@
+/* Copyright (c) 2010-2012 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/io.h>
+#include <linux/types.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+
+#include "mdss_hdmi_hdcp.h"
+
+#define HDCP_STATE_NAME (hdcp_state_name(hdcp_ctrl->hdcp_state))
+
+/* HDCP Keys state based on HDMI_HDCP_LINK0_STATUS:KEYS_STATE */
+#define HDCP_KEYS_STATE_NO_KEYS		0
+#define HDCP_KEYS_STATE_NOT_CHECKED	1
+#define HDCP_KEYS_STATE_CHECKING	2
+#define HDCP_KEYS_STATE_VALID		3
+#define HDCP_KEYS_STATE_AKSV_NOT_VALID	4
+#define HDCP_KEYS_STATE_CHKSUM_MISMATCH	5
+#define HDCP_KEYS_STATE_PROD_AKSV	6
+#define HDCP_KEYS_STATE_RESERVED	7
+
+struct hdmi_hdcp_ctrl {
+	enum hdmi_hdcp_state hdcp_state;
+	struct delayed_work hdcp_auth_work;
+	struct work_struct hdcp_int_work;
+	struct completion r0_checked;
+	struct hdmi_hdcp_init_data init_data;
+};
+
+const char *hdcp_state_name(enum hdmi_hdcp_state hdcp_state)
+{
+	switch (hdcp_state) {
+	case HDCP_STATE_INACTIVE:	return "HDCP_STATE_INACTIVE";
+	case HDCP_STATE_AUTHENTICATING:	return "HDCP_STATE_AUTHENTICATING";
+	case HDCP_STATE_AUTHENTICATED:	return "HDCP_STATE_AUTHENTICATED";
+	case HDCP_STATE_AUTH_FAIL:	return "HDCP_STATE_AUTH_FAIL";
+	default:			return "???";
+	}
+} /* hdcp_state_name */
+
+static int hdmi_hdcp_count_one(u8 *array, u8 len)
+{
+	int i, j, count = 0;
+	for (i = 0; i < len; i++)
+		for (j = 0; j < 8; j++)
+			count += (((array[i] >> j) & 0x1) ? 1 : 0);
+	return count;
+} /* hdmi_hdcp_count_one */
+
+static void reset_hdcp_ddc_failures(struct hdmi_hdcp_ctrl *hdcp_ctrl)
+{
+	int hdcp_ddc_ctrl1_reg;
+	int hdcp_ddc_status;
+	int failure;
+	int nack0;
+	struct dss_io_data *io;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+	/* Check for any DDC transfer failures */
+	hdcp_ddc_status = DSS_REG_R(io, HDMI_HDCP_DDC_STATUS);
+	failure = (hdcp_ddc_status >> 16) & 0x1;
+	nack0 = (hdcp_ddc_status >> 14) & 0x1;
+	DEV_DBG("%s: %s: On Entry: HDCP_DDC_STATUS=0x%x, FAIL=%d, NACK0=%d\n",
+		__func__, HDCP_STATE_NAME, hdcp_ddc_status, failure, nack0);
+
+	if (failure == 0x1) {
+		/*
+		 * Indicates that the last HDCP HW DDC transfer failed.
+		 * This occurs when a transfer is attempted with HDCP DDC
+		 * disabled (HDCP_DDC_DISABLE=1) or the number of retries
+		 * matches HDCP_DDC_RETRY_CNT.
+		 * Failure occured,  let's clear it.
+		 */
+		DEV_DBG("%s: %s: DDC failure detected.HDCP_DDC_STATUS=0x%08x\n",
+			 __func__, HDCP_STATE_NAME, hdcp_ddc_status);
+
+		/* First, Disable DDC */
+		DSS_REG_W(io, HDMI_HDCP_DDC_CTRL_0, BIT(0));
+
+		/* ACK the Failure to Clear it */
+		hdcp_ddc_ctrl1_reg = DSS_REG_R(io, HDMI_HDCP_DDC_CTRL_1);
+		DSS_REG_W(io, HDMI_HDCP_DDC_CTRL_1,
+			hdcp_ddc_ctrl1_reg | BIT(0));
+
+		/* Check if the FAILURE got Cleared */
+		hdcp_ddc_status = DSS_REG_R(io, HDMI_HDCP_DDC_STATUS);
+		hdcp_ddc_status = (hdcp_ddc_status >> 16) & BIT(0);
+		if (hdcp_ddc_status == 0x0)
+			DEV_DBG("%s: %s: HDCP DDC Failure cleared\n", __func__,
+				HDCP_STATE_NAME);
+		else
+			DEV_WARN("%s: %s: Unable to clear HDCP DDC Failure",
+				__func__, HDCP_STATE_NAME);
+
+		/* Re-Enable HDCP DDC */
+		DSS_REG_W(io, HDMI_HDCP_DDC_CTRL_0, 0);
+	}
+
+	if (nack0 == 0x1) {
+		DEV_DBG("%s: %s: Before: HDMI_DDC_SW_STATUS=0x%08x\n", __func__,
+			HDCP_STATE_NAME, DSS_REG_R(io, HDMI_DDC_SW_STATUS));
+		/* Reset HDMI DDC software status */
+		DSS_REG_W_ND(io, HDMI_DDC_CTRL,
+			DSS_REG_R(io, HDMI_DDC_CTRL) | BIT(3));
+		msleep(20);
+		DSS_REG_W_ND(io, HDMI_DDC_CTRL,
+			DSS_REG_R(io, HDMI_DDC_CTRL) & ~(BIT(3)));
+
+		/* Reset HDMI DDC Controller */
+		DSS_REG_W_ND(io, HDMI_DDC_CTRL,
+			DSS_REG_R(io, HDMI_DDC_CTRL) | BIT(1));
+		msleep(20);
+		DSS_REG_W_ND(io, HDMI_DDC_CTRL,
+			DSS_REG_R(io, HDMI_DDC_CTRL) & ~BIT(1));
+		DEV_DBG("%s: %s: After: HDMI_DDC_SW_STATUS=0x%08x\n", __func__,
+			HDCP_STATE_NAME, DSS_REG_R(io, HDMI_DDC_SW_STATUS));
+	}
+
+	hdcp_ddc_status = DSS_REG_R(io, HDMI_HDCP_DDC_STATUS);
+
+	failure = (hdcp_ddc_status >> 16) & BIT(0);
+	nack0 = (hdcp_ddc_status >> 14) & BIT(0);
+	DEV_DBG("%s: %s: On Exit: HDCP_DDC_STATUS=0x%x, FAIL=%d, NACK0=%d\n",
+		__func__, HDCP_STATE_NAME, hdcp_ddc_status, failure, nack0);
+} /* reset_hdcp_ddc_failures */
+
+static int hdmi_hdcp_authentication_part1(struct hdmi_hdcp_ctrl *hdcp_ctrl)
+{
+	int rc;
+	u32 qfprom_aksv_lsb, qfprom_aksv_msb;
+	u32 link0_aksv_0, link0_aksv_1;
+	u32 link0_bksv_0, link0_bksv_1;
+	u32 link0_an_0, link0_an_1;
+	u32 timeout_count;
+	bool is_match;
+	bool stale_an = false;
+	struct dss_io_data *io;
+	u8 aksv[5], bksv[5];
+	u8 an[8];
+	u8 bcaps;
+	struct hdmi_tx_ddc_data ddc_data;
+	u32 link0_status, an_ready, keys_state;
+	u8 buf[0xFF];
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io ||
+		!hdcp_ctrl->init_data.qfprom_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	if (HDCP_STATE_AUTHENTICATING != hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: invalid state. returning\n", __func__,
+			HDCP_STATE_NAME);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+	/* Fetch aksv from QFPROM, this info should be public. */
+	qfprom_aksv_lsb = DSS_REG_R(hdcp_ctrl->init_data.qfprom_io,
+		HDCP_KSV_LSB);
+	qfprom_aksv_msb = DSS_REG_R(hdcp_ctrl->init_data.qfprom_io,
+		HDCP_KSV_MSB);
+
+	aksv[0] =  qfprom_aksv_lsb        & 0xFF;
+	aksv[1] = (qfprom_aksv_lsb >> 8)  & 0xFF;
+	aksv[2] = (qfprom_aksv_lsb >> 16) & 0xFF;
+	aksv[3] = (qfprom_aksv_lsb >> 24) & 0xFF;
+	aksv[4] =  qfprom_aksv_msb        & 0xFF;
+
+	/* check there are 20 ones in AKSV */
+	if (hdmi_hdcp_count_one(aksv, 5) != 20) {
+		DEV_ERR("%s: %s: AKSV QFPROM doesn't have 20 1's, 20 0's\n",
+			__func__, HDCP_STATE_NAME);
+		DEV_ERR("%s: %s: QFPROM AKSV chk failed (AKSV=%02x%08x)\n",
+			__func__, HDCP_STATE_NAME, qfprom_aksv_msb,
+			qfprom_aksv_lsb);
+		rc = -EINVAL;
+		goto error;
+	}
+	DEV_DBG("%s: %s: AKSV=%02x%08x\n", __func__, HDCP_STATE_NAME,
+		qfprom_aksv_msb, qfprom_aksv_lsb);
+
+	/*
+	 * Write AKSV read from QFPROM to the HDCP registers.
+	 * This step is needed for HDCP authentication and must be
+	 * written before enabling HDCP.
+	 */
+	DSS_REG_W(io, HDMI_HDCP_SW_LOWER_AKSV, qfprom_aksv_lsb);
+	DSS_REG_W(io, HDMI_HDCP_SW_UPPER_AKSV, qfprom_aksv_msb);
+
+	/* Check to see if link0_Status has stale values for An ready bit */
+	link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+	DEV_DBG("%s: %s: Before enabling cipher Link0_status=0x%08x\n",
+		__func__, HDCP_STATE_NAME, link0_status);
+	if (link0_status & (BIT(8) | BIT(9))) {
+		DEV_DBG("%s: %s: An ready even before enabling HDCP\n",
+		__func__, HDCP_STATE_NAME);
+		stale_an = true;
+	}
+
+	/*
+	 * Read BCAPS
+	 * We need to first try to read an HDCP register on the sink to see if
+	 * the sink is ready for HDCP authentication
+	 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x40;
+	ddc_data.data_buf = &bcaps;
+	ddc_data.data_len = 1;
+	ddc_data.request_len = 1;
+	ddc_data.retry = 5;
+	ddc_data.what = "Bcaps";
+	ddc_data.no_align = true;
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: BCAPS read failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+	DEV_DBG("%s: %s: BCAPS=%02x\n", __func__, HDCP_STATE_NAME, bcaps);
+
+	/*
+	 * HDCP setup prior to enabling HDCP_CTRL.
+	 * Setup seed values for random number An.
+	 */
+	DSS_REG_W(io, HDMI_HDCP_ENTROPY_CTRL0, 0xB1FFB0FF);
+	DSS_REG_W(io, HDMI_HDCP_ENTROPY_CTRL1, 0xF00DFACE);
+
+	/* Disable the RngCipher state */
+	DSS_REG_W(io, HDMI_HDCP_DEBUG_CTRL,
+		DSS_REG_R(io, HDMI_HDCP_DEBUG_CTRL) & ~(BIT(2)));
+	DEV_DBG("%s: %s: HDCP_DEBUG_CTRL=0x%08x\n", __func__, HDCP_STATE_NAME,
+		DSS_REG_R(io, HDMI_HDCP_DEBUG_CTRL));
+
+	/* Ensure that all register writes are completed before
+	 * enabling HDCP cipher
+	 */
+	wmb();
+
+	/*
+	 * Enable HDCP
+	 * This needs to be done as early as possible in order for the
+	 * hardware to make An available to read
+	 */
+	DSS_REG_W(io, HDMI_HDCP_CTRL, BIT(0));
+
+	/* Clear any DDC failures from previous tries */
+	reset_hdcp_ddc_failures(hdcp_ctrl);
+
+	/* Write BCAPS to the hardware */
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA12, bcaps);
+
+	/*
+	 * If we had stale values for the An ready bit, it should most
+	 * likely be cleared now after enabling HDCP cipher
+	 */
+	link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+	DEV_DBG("%s: %s: After enabling HDCP Link0_Status=0x%08x\n",
+		__func__, HDCP_STATE_NAME, link0_status);
+	if (!(link0_status & (BIT(8) | BIT(9)))) {
+		DEV_DBG("%s: %s: An not ready after enabling HDCP\n",
+			__func__, HDCP_STATE_NAME);
+		stale_an = false;
+	}
+
+	/* Wait for HDCP keys to be checked and validated */
+	timeout_count = 100;
+	keys_state = (link0_status >> 28) & 0x7;
+	while ((keys_state != HDCP_KEYS_STATE_VALID) &&
+		timeout_count--) {
+		link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+		keys_state = (link0_status >> 28) & 0x7;
+		DEV_DBG("%s: %s: Keys not ready(%d). s=%d\n, l0=%0x08x",
+			__func__, HDCP_STATE_NAME, timeout_count,
+			keys_state, link0_status);
+		msleep(20);
+	}
+
+	if (!timeout_count) {
+		DEV_ERR("%s: %s: Invalid Keys State: %d\n", __func__,
+			HDCP_STATE_NAME, keys_state);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * 1.1_Features turned off by default.
+	 * No need to write AInfo since 1.1_Features is disabled.
+	 */
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA4, 0);
+
+	/* Wait for An0 and An1 bit to be ready */
+	timeout_count = 100;
+	do {
+		link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+		an_ready = (link0_status & BIT(8)) && (link0_status & BIT(9));
+		if (!an_ready) {
+			DEV_DBG("%s: %s: An not ready(%d). l0_status=0x%08x\n",
+				__func__, HDCP_STATE_NAME, timeout_count,
+				link0_status);
+			msleep(20);
+		}
+	} while (!an_ready && timeout_count--);
+
+	if (!timeout_count) {
+		rc = -ETIMEDOUT;
+		DEV_ERR("%s: %s: timedout, An0=%ld, An1=%ld\n", __func__,
+			HDCP_STATE_NAME, (link0_status & BIT(8)) >> 8,
+			(link0_status & BIT(9)) >> 9);
+		goto error;
+	}
+
+	/*
+	 * In cases where An_ready bits had stale values, it would be
+	 * better to delay reading of An to avoid any potential of this
+	 * read being blocked
+	 */
+	if (stale_an) {
+		msleep(200);
+		stale_an = false;
+	}
+
+	/* Read An0 and An1 */
+	link0_an_0 = DSS_REG_R(io, HDMI_HDCP_RCVPORT_DATA5);
+	link0_an_1 = DSS_REG_R(io, HDMI_HDCP_RCVPORT_DATA6);
+
+	/* Read AKSV */
+	link0_aksv_0 = DSS_REG_R(io, HDMI_HDCP_RCVPORT_DATA3);
+	link0_aksv_1 = DSS_REG_R(io, HDMI_HDCP_RCVPORT_DATA4);
+
+	/* Copy An and AKSV to byte arrays for transmission */
+	aksv[0] =  link0_aksv_0        & 0xFF;
+	aksv[1] = (link0_aksv_0 >> 8)  & 0xFF;
+	aksv[2] = (link0_aksv_0 >> 16) & 0xFF;
+	aksv[3] = (link0_aksv_0 >> 24) & 0xFF;
+	aksv[4] =  link0_aksv_1        & 0xFF;
+
+	an[0] =  link0_an_0        & 0xFF;
+	an[1] = (link0_an_0 >> 8)  & 0xFF;
+	an[2] = (link0_an_0 >> 16) & 0xFF;
+	an[3] = (link0_an_0 >> 24) & 0xFF;
+	an[4] =  link0_an_1        & 0xFF;
+	an[5] = (link0_an_1 >> 8)  & 0xFF;
+	an[6] = (link0_an_1 >> 16) & 0xFF;
+	an[7] = (link0_an_1 >> 24) & 0xFF;
+
+	/* Write An to offset 0x18 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x18;
+	ddc_data.data_buf = an;
+	ddc_data.data_len = 8;
+	ddc_data.what = "An";
+	rc = hdmi_ddc_write(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: An write failed\n", __func__, HDCP_STATE_NAME);
+		goto error;
+	}
+
+	/* Write AKSV to offset 0x10 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x10;
+	ddc_data.data_buf = aksv;
+	ddc_data.data_len = 5;
+	ddc_data.what = "Aksv";
+	rc = hdmi_ddc_write(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: AKSV write failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+	DEV_DBG("%s: %s: Link0-AKSV=%02x%08x\n", __func__,
+		HDCP_STATE_NAME, link0_aksv_1 & 0xFF, link0_aksv_0);
+
+	/* Read BKSV at offset 0x00 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x00;
+	ddc_data.data_buf = bksv;
+	ddc_data.data_len = 5;
+	ddc_data.request_len = 5;
+	ddc_data.retry = 5;
+	ddc_data.what = "Bksv";
+	ddc_data.no_align = true;
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: BKSV read failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+
+	/* check there are 20 ones in BKSV */
+	if (hdmi_hdcp_count_one(bksv, 5) != 20) {
+		DEV_ERR("%s: %s: BKSV doesn't have 20 1's and 20 0's\n",
+			__func__, HDCP_STATE_NAME);
+		DEV_ERR("%s: %s: BKSV chk fail. BKSV=%02x%02x%02x%02x%02x\n",
+			__func__, HDCP_STATE_NAME, bksv[4], bksv[3], bksv[2],
+			bksv[1], bksv[0]);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	link0_bksv_0 = bksv[3];
+	link0_bksv_0 = (link0_bksv_0 << 8) | bksv[2];
+	link0_bksv_0 = (link0_bksv_0 << 8) | bksv[1];
+	link0_bksv_0 = (link0_bksv_0 << 8) | bksv[0];
+	link0_bksv_1 = bksv[4];
+	DEV_DBG("%s: %s: BKSV=%02x%08x\n", __func__, HDCP_STATE_NAME,
+		link0_bksv_1, link0_bksv_0);
+
+	/* Write BKSV read from sink to HDCP registers */
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA0, link0_bksv_0);
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA1, link0_bksv_1);
+
+	/* Enable HDCP interrupts and ack/clear any stale interrupts */
+	DSS_REG_W(io, HDMI_HDCP_INT_CTRL, 0xE6);
+
+	/*
+	 * HDCP Compliace Test case 1A-01:
+	 * Wait here at least 100ms before reading R0'
+	 */
+	msleep(125);
+
+	/* Read R0' at offset 0x08 */
+	memset(buf, 0, sizeof(buf));
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x08;
+	ddc_data.data_buf = buf;
+	ddc_data.data_len = 2;
+	ddc_data.request_len = 2;
+	ddc_data.retry = 5;
+	ddc_data.what = "R0'";
+	ddc_data.no_align = true;
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: R0' read failed\n", __func__, HDCP_STATE_NAME);
+		goto error;
+	}
+	DEV_DBG("%s: %s: R0'=%02x%02x\n", __func__, HDCP_STATE_NAME,
+		buf[1], buf[0]);
+
+	/* Write R0' to HDCP registers and check to see if it is a match */
+	INIT_COMPLETION(hdcp_ctrl->r0_checked);
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA2_0, (((u32)buf[1]) << 8) | buf[0]);
+	timeout_count = wait_for_completion_interruptible_timeout(
+		&hdcp_ctrl->r0_checked, HZ*2);
+	link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+	is_match = link0_status & BIT(12);
+	if (!is_match) {
+		DEV_DBG("%s: %s: Link0_Status=0x%08x\n", __func__,
+			HDCP_STATE_NAME, link0_status);
+		if (!timeout_count) {
+			DEV_ERR("%s: %s: Timeout. No R0 mtch. R0'=%02x%02x\n",
+				__func__, HDCP_STATE_NAME, buf[1], buf[0]);
+			rc = -ETIMEDOUT;
+			goto error;
+		} else {
+			DEV_ERR("%s: %s: R0 mismatch. R0'=%02x%02x\n", __func__,
+				HDCP_STATE_NAME, buf[1], buf[0]);
+			rc = -EINVAL;
+			goto error;
+		}
+	} else {
+		DEV_DBG("%s: %s: R0 matches\n", __func__, HDCP_STATE_NAME);
+	}
+
+error:
+	if (rc) {
+		DEV_ERR("%s: %s: Authentication Part I failed\n", __func__,
+			HDCP_STATE_NAME);
+	} else {
+		/* Enable HDCP Encryption */
+		DSS_REG_W(io, HDMI_HDCP_CTRL, BIT(0) | BIT(8));
+		DEV_INFO("%s: %s: Authentication Part I successful\n",
+			__func__, HDCP_STATE_NAME);
+	}
+	return rc;
+} /* hdmi_hdcp_authentication_part1 */
+
+#define READ_WRITE_V_H(off, name, reg) \
+do { \
+	ddc_data.offset = (off); \
+	memset(what, 0, sizeof(what)); \
+	snprintf(what, sizeof(what), (name)); \
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data); \
+	if (rc) { \
+		DEV_ERR("%s: %s: Read %s failed\n", __func__, HDCP_STATE_NAME, \
+			what); \
+		goto error; \
+	} \
+	DEV_DBG("%s: %s: %s: buf[0]=%x, buf[1]=%x, buf[2]=%x, buf[3]=%x\n", \
+			__func__, HDCP_STATE_NAME, what, buf[0], buf[1], \
+			buf[2], buf[3]); \
+	DSS_REG_W(io, (reg), \
+			(buf[3] << 24 | buf[2] << 16 | buf[1] << 8 | buf[0])); \
+} while (0);
+
+static int hdmi_hdcp_transfer_v_h(struct hdmi_hdcp_ctrl *hdcp_ctrl)
+{
+	char what[20];
+	int rc = 0;
+	u8 buf[4];
+	struct hdmi_tx_ddc_data ddc_data;
+	struct dss_io_data *io;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -EINVAL;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.data_buf = buf;
+	ddc_data.data_len = 4;
+	ddc_data.request_len = 4;
+	ddc_data.retry = 5;
+	ddc_data.what = what;
+	ddc_data.no_align = true;
+
+	/* Read V'.HO 4 Byte at offset 0x20 */
+	READ_WRITE_V_H(0x20, "V' H0", HDMI_HDCP_RCVPORT_DATA7);
+
+	/* Read V'.H1 4 Byte at offset 0x24 */
+	READ_WRITE_V_H(0x24, "V' H1", HDMI_HDCP_RCVPORT_DATA8);
+
+	/* Read V'.H2 4 Byte at offset 0x28 */
+	READ_WRITE_V_H(0x28, "V' H2", HDMI_HDCP_RCVPORT_DATA9);
+
+	/* Read V'.H3 4 Byte at offset 0x2C */
+	READ_WRITE_V_H(0x2C, "V' H3", HDMI_HDCP_RCVPORT_DATA10);
+
+	/* Read V'.H4 4 Byte at offset 0x30 */
+	READ_WRITE_V_H(0x30, "V' H4", HDMI_HDCP_RCVPORT_DATA11);
+
+error:
+	return rc;
+}
+
+static int hdmi_hdcp_authentication_part2(struct hdmi_hdcp_ctrl *hdcp_ctrl)
+{
+	int rc, cnt, i;
+	struct hdmi_tx_ddc_data ddc_data;
+	u32 timeout_count, down_stream_devices;
+	u8 buf[0xFF];
+	u8 ksv_fifo[5 * 127];
+	u8 bcaps;
+	u16 bstatus, max_devs_exceeded, max_cascade_exceeded;
+	u32 link0_status;
+	u32 ksv_bytes;
+	struct dss_io_data *io;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	if (HDCP_STATE_AUTHENTICATING != hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: invalid state. returning\n", __func__,
+			HDCP_STATE_NAME);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+	memset(buf, 0, sizeof(buf));
+	memset(ksv_fifo, 0, sizeof(ksv_fifo));
+
+	/* Read BCAPS at offset 0x40 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x40;
+	ddc_data.data_buf = &bcaps;
+	ddc_data.data_len = 1;
+	ddc_data.request_len = 1;
+	ddc_data.retry = 5;
+	ddc_data.what = "Bcaps";
+	ddc_data.no_align = false;
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: BCAPS read failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+	DEV_DBG("%s: %s: BCAPS=%02x (%s)\n", __func__, HDCP_STATE_NAME, bcaps,
+		(bcaps & BIT(6)) ? "repeater" : "no repeater");
+
+	/* if REPEATER (Bit 6), perform Part2 Authentication */
+	if (!(bcaps & BIT(6))) {
+		DEV_INFO("%s: %s: auth part II skipped, no repeater\n",
+			__func__, HDCP_STATE_NAME);
+		return 0;
+	}
+
+	/* Wait until READY bit is set in BCAPS */
+	timeout_count = 50;
+	while (!(bcaps && BIT(5)) && timeout_count) {
+		msleep(100);
+		timeout_count--;
+		/* Read BCAPS at offset 0x40 */
+		memset(&ddc_data, 0, sizeof(ddc_data));
+		ddc_data.dev_addr = 0x74;
+		ddc_data.offset = 0x40;
+		ddc_data.data_buf = &bcaps;
+		ddc_data.data_len = 1;
+		ddc_data.request_len = 1;
+		ddc_data.retry = 5;
+		ddc_data.what = "Bcaps";
+		ddc_data.no_align = false;
+		rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+		if (rc) {
+			DEV_ERR("%s: %s: BCAPS read failed\n", __func__,
+				HDCP_STATE_NAME);
+			goto error;
+		}
+	}
+
+	/* Read BSTATUS at offset 0x41 */
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x41;
+	ddc_data.data_buf = buf;
+	ddc_data.data_len = 2;
+	ddc_data.request_len = 2;
+	ddc_data.retry = 5;
+	ddc_data.what = "Bstatuss";
+	ddc_data.no_align = false;
+	rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+	if (rc) {
+		DEV_ERR("%s: %s: BSTATUS read failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+	bstatus = buf[1];
+	bstatus = (bstatus << 8) | buf[0];
+
+	/* Write BSTATUS and BCAPS to HDCP registers */
+	DSS_REG_W(io, HDMI_HDCP_RCVPORT_DATA12, bcaps | (bstatus << 8));
+
+	down_stream_devices = bstatus & 0x7F;
+	if (down_stream_devices == 0) {
+		/*
+		 * If no downstream devices are attached to the repeater
+		 * then part II fails.
+		 * todo: The other approach would be to continue PART II.
+		 */
+		DEV_ERR("%s: %s: No downstream devices\n", __func__,
+			HDCP_STATE_NAME);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * HDCP Compliance 1B-05:
+	 * Check if no. of devices connected to repeater
+	 * exceed max_devices_connected from bit 7 of Bstatus.
+	 */
+	max_devs_exceeded = (bstatus & BIT(7)) >> 7;
+	if (max_devs_exceeded == 0x01) {
+		DEV_ERR("%s: %s: no. of devs connected exceeds max allowed",
+			__func__, HDCP_STATE_NAME);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * HDCP Compliance 1B-06:
+	 * Check if no. of cascade connected to repeater
+	 * exceed max_cascade_connected from bit 11 of Bstatus.
+	 */
+	max_cascade_exceeded = (bstatus & BIT(11)) >> 11;
+	if (max_cascade_exceeded == 0x01) {
+		DEV_ERR("%s: %s: no. of cascade conn exceeds max allowed",
+			__func__, HDCP_STATE_NAME);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	/*
+	 * Read KSV FIFO over DDC
+	 * Key Slection vector FIFO Used to pull downstream KSVs
+	 * from HDCP Repeaters.
+	 * All bytes (DEVICE_COUNT * 5) must be read in a single,
+	 * auto incrementing access.
+	 * All bytes read as 0x00 for HDCP Receivers that are not
+	 * HDCP Repeaters (REPEATER == 0).
+	 */
+	ksv_bytes = 5 * down_stream_devices;
+	memset(&ddc_data, 0, sizeof(ddc_data));
+	ddc_data.dev_addr = 0x74;
+	ddc_data.offset = 0x43;
+	ddc_data.data_buf = ksv_fifo;
+	ddc_data.data_len = ksv_bytes;
+	ddc_data.request_len = ksv_bytes;
+	ddc_data.retry = 5;
+	ddc_data.what = "KSV FIFO";
+	ddc_data.no_align = true;
+	cnt = 0;
+	do {
+		rc = hdmi_ddc_read(hdcp_ctrl->init_data.ddc_ctrl, &ddc_data);
+		if (rc) {
+			DEV_ERR("%s: %s: KSV FIFO read failed\n", __func__,
+				HDCP_STATE_NAME);
+			/*
+			 * HDCP Compliace Test case 1B-01:
+			 * Wait here until all the ksv bytes have been
+			 * read from the KSV FIFO register.
+			 */
+			msleep(25);
+		} else {
+			break;
+		}
+		cnt++;
+	} while (cnt != 20);
+
+	if (cnt == 20)
+		goto error;
+
+	rc = hdmi_hdcp_transfer_v_h(hdcp_ctrl);
+	if (rc)
+		goto error;
+
+	/*
+	 * Write KSV FIFO to HDCP_SHA_DATA.
+	 * This is done 1 byte at time starting with the LSB.
+	 * On the very last byte write, the HDCP_SHA_DATA_DONE bit[0]
+	 */
+
+	/* First, reset SHA engine */
+	DSS_REG_W(io, HDMI_HDCP_SHA_CTRL, 1);
+	/* Next, enable SHA engine, SEL=DIGA_HDCP */
+	DSS_REG_W(io, HDMI_HDCP_SHA_CTRL, 0);
+
+	for (i = 0; i < ksv_bytes - 1; i++) {
+		/* Write KSV byte and do not set DONE bit[0] */
+		DSS_REG_W_ND(io, HDMI_HDCP_SHA_DATA, ksv_fifo[i] << 16);
+
+		/*
+		 * Once 64 bytes have been written, we need to poll for
+		 * HDCP_SHA_BLOCK_DONE before writing any further
+		 */
+		if (i && !((i + 1) % 64)) {
+			timeout_count = 100;
+			while (!(DSS_REG_R(io, HDMI_HDCP_SHA_STATUS) & BIT(0))
+				&& (--timeout_count)) {
+				DEV_DBG("%s: %s: Wrote 64 bytes KVS FIFO\n",
+					__func__, HDCP_STATE_NAME);
+				DEV_DBG("%s: %s: HDCP_SHA_STATUS=%08x\n",
+					__func__, HDCP_STATE_NAME,
+					DSS_REG_R(io, HDMI_HDCP_SHA_STATUS));
+				msleep(20);
+			}
+			if (!timeout_count) {
+				rc = -ETIMEDOUT;
+				DEV_ERR("%s: %s: Write KSV FIFO timedout",
+					__func__, HDCP_STATE_NAME);
+				goto error;
+			}
+		}
+
+	}
+
+	/* Write l to DONE bit[0] */
+	DSS_REG_W_ND(io, HDMI_HDCP_SHA_DATA,
+			(ksv_fifo[ksv_bytes - 1] << 16) | 0x1);
+
+	/* Now wait for HDCP_SHA_COMP_DONE */
+	timeout_count = 100;
+	while ((0x10 != (DSS_REG_R(io, HDMI_HDCP_SHA_STATUS)
+		& 0xFFFFFF10)) && --timeout_count)
+		msleep(20);
+	if (!timeout_count) {
+		rc = -ETIMEDOUT;
+		DEV_ERR("%s: %s: SHA computation timedout", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+
+	/* Wait for V_MATCHES */
+	timeout_count = 100;
+	link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+	while (((link0_status & BIT(20)) != BIT(20)) && --timeout_count) {
+		DEV_DBG("%s: %s: Waiting for V_MATCHES(%d). l0_status=0x%08x\n",
+			__func__, HDCP_STATE_NAME, timeout_count, link0_status);
+		msleep(20);
+		link0_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+	}
+	if (!timeout_count) {
+		rc = -ETIMEDOUT;
+		DEV_ERR("%s: %s: HDCP V Match timedout", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+
+error:
+	if (rc)
+		DEV_ERR("%s: %s: Authentication Part II failed\n", __func__,
+			HDCP_STATE_NAME);
+	else
+		DEV_INFO("%s: %s: Authentication Part II successful\n",
+			__func__, HDCP_STATE_NAME);
+	return rc;
+} /* hdmi_hdcp_authentication_part2 */
+
+static void hdmi_hdcp_int_work(struct work_struct *work)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = container_of(work,
+		struct hdmi_hdcp_ctrl, hdcp_int_work);
+
+	if (!hdcp_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	mutex_lock(hdcp_ctrl->init_data.mutex);
+	hdcp_ctrl->hdcp_state = HDCP_STATE_AUTH_FAIL;
+	mutex_unlock(hdcp_ctrl->init_data.mutex);
+
+	if (hdcp_ctrl->init_data.notify_status) {
+		hdcp_ctrl->init_data.notify_status(
+			hdcp_ctrl->init_data.cb_data,
+			hdcp_ctrl->hdcp_state);
+	}
+} /* hdmi_hdcp_int_work */
+
+static void hdmi_hdcp_auth_work(struct work_struct *work)
+{
+	int rc;
+	struct delayed_work *dw = to_delayed_work(work);
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = container_of(dw,
+		struct hdmi_hdcp_ctrl, hdcp_auth_work);
+
+	if (!hdcp_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	if (HDCP_STATE_AUTHENTICATING != hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: invalid state. returning\n", __func__,
+			HDCP_STATE_NAME);
+		return;
+	}
+
+	rc = hdmi_hdcp_authentication_part1(hdcp_ctrl);
+	if (rc) {
+		DEV_DBG("%s: %s: HDCP Auth Part I failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+
+	rc = hdmi_hdcp_authentication_part2(hdcp_ctrl);
+	if (rc) {
+		DEV_DBG("%s: %s: HDCP Auth Part II failed\n", __func__,
+			HDCP_STATE_NAME);
+		goto error;
+	}
+
+error:
+	/*
+	 * Ensure that the state did not change during authentication.
+	 * If it did, it means that deauthenticate/reauthenticate was
+	 * called. In that case, this function need not notify HDMI Tx
+	 * of the result
+	 */
+	mutex_lock(hdcp_ctrl->init_data.mutex);
+	if (HDCP_STATE_AUTHENTICATING == hdcp_ctrl->hdcp_state) {
+		if (rc)
+			hdcp_ctrl->hdcp_state = HDCP_STATE_AUTH_FAIL;
+		else
+			hdcp_ctrl->hdcp_state = HDCP_STATE_AUTHENTICATED;
+		mutex_unlock(hdcp_ctrl->init_data.mutex);
+
+		/* Notify HDMI Tx controller of the result */
+		DEV_DBG("%s: %s: Notifying HDMI Tx of auth result\n",
+			__func__, HDCP_STATE_NAME);
+		if (hdcp_ctrl->init_data.notify_status) {
+			hdcp_ctrl->init_data.notify_status(
+				hdcp_ctrl->init_data.cb_data,
+				hdcp_ctrl->hdcp_state);
+		}
+	} else {
+		DEV_DBG("%s: %s: HDCP state changed during authentication\n",
+			__func__, HDCP_STATE_NAME);
+		mutex_unlock(hdcp_ctrl->init_data.mutex);
+	}
+	return;
+} /* hdmi_hdcp_auth_work */
+
+int hdmi_hdcp_authenticate(void *input)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = (struct hdmi_hdcp_ctrl *)input;
+
+	if (!hdcp_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -EINVAL;
+	}
+
+	if (HDCP_STATE_INACTIVE != hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: already active or activating. returning\n",
+			__func__, HDCP_STATE_NAME);
+		return 0;
+	}
+
+	DEV_DBG("%s: %s: Queuing work to start HDCP authentication", __func__,
+		HDCP_STATE_NAME);
+	mutex_lock(hdcp_ctrl->init_data.mutex);
+	hdcp_ctrl->hdcp_state = HDCP_STATE_AUTHENTICATING;
+	mutex_unlock(hdcp_ctrl->init_data.mutex);
+	queue_delayed_work(hdcp_ctrl->init_data.workq,
+		&hdcp_ctrl->hdcp_auth_work, 0);
+
+	return 0;
+} /* hdmi_hdcp_authenticate */
+
+int hdmi_hdcp_reauthenticate(void *input)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = (struct hdmi_hdcp_ctrl *)input;
+	struct dss_io_data *io;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -EINVAL;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+
+	if (HDCP_STATE_AUTH_FAIL != hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: invalid state. returning\n", __func__,
+			HDCP_STATE_NAME);
+		return 0;
+	}
+
+	/*
+	 * Disable HPD circuitry.
+	 * This is needed to reset the HDCP cipher engine so that when we
+	 * attempt a re-authentication, HW would clear the AN0_READY and
+	 * AN1_READY bits in HDMI_HDCP_LINK0_STATUS register
+	 */
+	DSS_REG_W(io, HDMI_HPD_CTRL, DSS_REG_R(hdcp_ctrl->init_data.core_io,
+		HDMI_HPD_CTRL) & ~BIT(28));
+
+	/* Disable HDCP interrupts */
+	DSS_REG_W(io, HDMI_HDCP_INT_CTRL, 0);
+
+	DSS_REG_W(io, HDMI_HDCP_RESET, BIT(0));
+
+	/* Disable encryption and disable the HDCP block */
+	DSS_REG_W(io, HDMI_HDCP_CTRL, 0);
+
+	/* Enable HPD circuitry */
+	DSS_REG_W(hdcp_ctrl->init_data.core_io, HDMI_HPD_CTRL,
+		DSS_REG_R(hdcp_ctrl->init_data.core_io,
+		HDMI_HPD_CTRL) | BIT(28));
+
+	/* Restart authentication attempt */
+	DEV_DBG("%s: %s: Scheduling work to start HDCP authentication",
+		__func__, HDCP_STATE_NAME);
+	mutex_lock(hdcp_ctrl->init_data.mutex);
+	hdcp_ctrl->hdcp_state = HDCP_STATE_AUTHENTICATING;
+	mutex_unlock(hdcp_ctrl->init_data.mutex);
+	queue_delayed_work(hdcp_ctrl->init_data.workq,
+		&hdcp_ctrl->hdcp_auth_work, HZ/2);
+
+	return 0;
+} /* hdmi_hdcp_reauthenticate */
+
+void hdmi_hdcp_off(void *input)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = (struct hdmi_hdcp_ctrl *)input;
+	struct dss_io_data *io;
+	int rc = 0;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+	if (HDCP_STATE_INACTIVE == hdcp_ctrl->hdcp_state) {
+		DEV_DBG("%s: %s: inactive. returning\n", __func__,
+			HDCP_STATE_NAME);
+		return;
+	}
+
+	/*
+	 * Need to set the state to inactive here so that any ongoing
+	 * reauth works will know that the HDCP session has been turned off
+	 */
+	mutex_lock(hdcp_ctrl->init_data.mutex);
+	hdcp_ctrl->hdcp_state = HDCP_STATE_INACTIVE;
+	mutex_unlock(hdcp_ctrl->init_data.mutex);
+
+	/* Disable HDCP interrupts */
+	DSS_REG_W(io, HDMI_HDCP_INT_CTRL, 0);
+
+	/*
+	 * Cancel any pending auth/reauth attempts.
+	 * If one is ongoing, this will wait for it to finish.
+	 * No more reauthentiaction attempts will be scheduled since we
+	 * set the currect state to inactive.
+	 */
+	rc = cancel_delayed_work_sync(&hdcp_ctrl->hdcp_auth_work);
+	if (rc)
+		DEV_DBG("%s: %s: Deleted hdcp auth work\n", __func__,
+			HDCP_STATE_NAME);
+	rc = cancel_work_sync(&hdcp_ctrl->hdcp_int_work);
+	if (rc)
+		DEV_DBG("%s: %s: Deleted hdcp int work\n", __func__,
+			HDCP_STATE_NAME);
+
+	DSS_REG_W(io, HDMI_HDCP_RESET, BIT(0));
+
+	/* Disable encryption and disable the HDCP block */
+	DSS_REG_W(io, HDMI_HDCP_CTRL, 0);
+
+	DEV_DBG("%s: %s: HDCP: Off\n", __func__, HDCP_STATE_NAME);
+} /* hdmi_hdcp_off */
+
+int hdmi_hdcp_isr(void *input)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = (struct hdmi_hdcp_ctrl *)input;
+	int rc = 0;
+	struct dss_io_data *io;
+	u32 hdcp_int_val;
+
+	if (!hdcp_ctrl || !hdcp_ctrl->init_data.core_io) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		rc = -EINVAL;
+		goto error;
+	}
+
+	io = hdcp_ctrl->init_data.core_io;
+
+	/* Ignore HDCP interrupts if HDCP is disabled */
+	if (HDCP_STATE_INACTIVE == hdcp_ctrl->hdcp_state)
+		return 0;
+
+	hdcp_int_val = DSS_REG_R(io, HDMI_HDCP_INT_CTRL);
+	if (hdcp_int_val & BIT(0)) {
+		/* AUTH_SUCCESS_INT */
+		DSS_REG_W(io, HDMI_HDCP_INT_CTRL, (hdcp_int_val | BIT(1)));
+		DEV_INFO("%s: %s: AUTH_SUCCESS_INT received\n", __func__,
+			HDCP_STATE_NAME);
+		if (HDCP_STATE_AUTHENTICATING == hdcp_ctrl->hdcp_state)
+			complete_all(&hdcp_ctrl->r0_checked);
+	}
+
+	if (hdcp_int_val & BIT(4)) {
+		/* AUTH_FAIL_INT */
+		u32 link_status = DSS_REG_R(io, HDMI_HDCP_LINK0_STATUS);
+		DSS_REG_W(io, HDMI_HDCP_INT_CTRL, (hdcp_int_val | BIT(5)));
+		DEV_INFO("%s: %s: AUTH_FAIL_INT rcvd, LINK0_STATUS=0x%08x\n",
+			__func__, HDCP_STATE_NAME, link_status);
+		if (HDCP_STATE_AUTHENTICATED == hdcp_ctrl->hdcp_state) {
+			/* Inform HDMI Tx of the failure */
+			queue_work(hdcp_ctrl->init_data.workq,
+				&hdcp_ctrl->hdcp_int_work);
+			/* todo: print debug log with auth fail reason */
+		} else if (HDCP_STATE_AUTHENTICATING == hdcp_ctrl->hdcp_state) {
+			complete_all(&hdcp_ctrl->r0_checked);
+		}
+
+		/* Clear AUTH_FAIL_INFO as well */
+		DSS_REG_W(io, HDMI_HDCP_INT_CTRL, (hdcp_int_val | BIT(7)));
+	}
+
+	if (hdcp_int_val & BIT(8)) {
+		/* DDC_XFER_REQ_INT */
+		DSS_REG_W(io, HDMI_HDCP_INT_CTRL, (hdcp_int_val | BIT(9)));
+		DEV_INFO("%s: %s: DDC_XFER_REQ_INT received\n", __func__,
+			HDCP_STATE_NAME);
+	}
+
+	if (hdcp_int_val & BIT(12)) {
+		/* DDC_XFER_DONE_INT */
+		DSS_REG_W(io, HDMI_HDCP_INT_CTRL, (hdcp_int_val | BIT(13)));
+		DEV_INFO("%s: %s: DDC_XFER_DONE received\n", __func__,
+			HDCP_STATE_NAME);
+	}
+
+error:
+	return rc;
+} /* hdmi_hdcp_isr */
+
+void hdmi_hdcp_deinit(void *input)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = (struct hdmi_hdcp_ctrl *)input;
+
+	if (!hdcp_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	kfree(hdcp_ctrl);
+} /* hdmi_hdcp_deinit */
+
+void *hdmi_hdcp_init(struct hdmi_hdcp_init_data *init_data)
+{
+	struct hdmi_hdcp_ctrl *hdcp_ctrl = NULL;
+
+	if (!init_data || !init_data->core_io || !init_data->qfprom_io ||
+		!init_data->mutex || !init_data->ddc_ctrl ||
+		!init_data->notify_status || !init_data->workq ||
+		!init_data->cb_data) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		goto error;
+	}
+
+	hdcp_ctrl = kzalloc(sizeof(*hdcp_ctrl), GFP_KERNEL);
+	if (!hdcp_ctrl) {
+		DEV_ERR("%s: Out of memory\n", __func__);
+		goto error;
+	}
+
+	hdcp_ctrl->init_data = *init_data;
+
+	INIT_DELAYED_WORK(&hdcp_ctrl->hdcp_auth_work, hdmi_hdcp_auth_work);
+	INIT_WORK(&hdcp_ctrl->hdcp_int_work, hdmi_hdcp_int_work);
+
+	hdcp_ctrl->hdcp_state = HDCP_STATE_INACTIVE;
+	init_completion(&hdcp_ctrl->r0_checked);
+	DEV_DBG("%s: HDCP module initialized. HDCP_STATE=%s", __func__,
+		HDCP_STATE_NAME);
+
+error:
+	return (void *)hdcp_ctrl;
+} /* hdmi_hdcp_init */
diff --git a/drivers/video/msm/mdss/mdss_hdmi_hdcp.h b/drivers/video/msm/mdss/mdss_hdmi_hdcp.h
new file mode 100644
index 0000000..d35b2a9
--- /dev/null
+++ b/drivers/video/msm/mdss/mdss_hdmi_hdcp.h
@@ -0,0 +1,43 @@
+/* Copyright (c) 2012 The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef __MDSS_HDMI_HDCP_H__
+#define __MDSS_HDMI_HDCP_H__
+
+#include "mdss_hdmi_util.h"
+
+enum hdmi_hdcp_state {
+	HDCP_STATE_INACTIVE,
+	HDCP_STATE_AUTHENTICATING,
+	HDCP_STATE_AUTHENTICATED,
+	HDCP_STATE_AUTH_FAIL
+};
+
+struct hdmi_hdcp_init_data {
+	struct dss_io_data *core_io;
+	struct dss_io_data *qfprom_io;
+	struct mutex *mutex;
+	struct workqueue_struct *workq;
+	void *cb_data;
+	void (*notify_status)(void *cb_data, enum hdmi_hdcp_state status);
+
+	struct hdmi_tx_ddc_ctrl *ddc_ctrl;
+};
+
+const char *hdcp_state_name(enum hdmi_hdcp_state hdcp_state);
+void *hdmi_hdcp_init(struct hdmi_hdcp_init_data *init_data);
+void hdmi_hdcp_deinit(void *input);
+int hdmi_hdcp_isr(void *ptr);
+int hdmi_hdcp_reauthenticate(void *input);
+int hdmi_hdcp_authenticate(void *hdcp_ctrl);
+void hdmi_hdcp_off(void *hdcp_ctrl);
+#endif /* __MDSS_HDMI_HDCP_H__ */
diff --git a/drivers/video/msm/mdss/mdss_hdmi_tx.c b/drivers/video/msm/mdss/mdss_hdmi_tx.c
index 37bbbdf..edc3634 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_tx.c
+++ b/drivers/video/msm/mdss/mdss_hdmi_tx.c
@@ -18,21 +18,23 @@
 #include <linux/iopoll.h>
 #include <linux/of_address.h>
 #include <linux/of_gpio.h>
+#include <linux/of_platform.h>
 #include <linux/types.h>
-#include <mach/msm_hdmi_audio.h>
+#include <mach/msm_hdmi_audio_codec.h>
 
 #define REG_DUMP 0
 
 #include "mdss_fb.h"
 #include "mdss_hdmi_tx.h"
 #include "mdss_hdmi_edid.h"
+#include "mdss_hdmi_hdcp.h"
 #include "mdss.h"
 #include "mdss_panel.h"
 
 #define DRV_NAME "hdmi-tx"
 #define COMPATIBLE_NAME "qcom,hdmi-tx"
 
-#define DEFAULT_VIDEO_RESOLUTION HDMI_VFRMT_1920x1080p60_16_9;
+#define DEFAULT_VIDEO_RESOLUTION HDMI_VFRMT_1920x1080p60_16_9
 
 /* HDMI PHY/PLL bit field macros */
 #define SW_RESET BIT(2)
@@ -45,6 +47,26 @@
 	((d & 0xff) + ((d >> 8) & 0xff) +	\
 	((d >> 16) & 0xff) + ((d >> 24) & 0xff))
 
+/* Enable HDCP by default */
+static bool hdcp_feature_on = true;
+
+/* Supported HDMI Audio channels */
+#define MSM_HDMI_AUDIO_CHANNEL_2	2
+#define MSM_HDMI_AUDIO_CHANNEL_4	4
+#define MSM_HDMI_AUDIO_CHANNEL_6	6
+#define MSM_HDMI_AUDIO_CHANNEL_8	8
+
+enum msm_hdmi_supported_audio_sample_rates {
+	AUDIO_SAMPLE_RATE_32KHZ,
+	AUDIO_SAMPLE_RATE_44_1KHZ,
+	AUDIO_SAMPLE_RATE_48KHZ,
+	AUDIO_SAMPLE_RATE_88_2KHZ,
+	AUDIO_SAMPLE_RATE_96KHZ,
+	AUDIO_SAMPLE_RATE_176_4KHZ,
+	AUDIO_SAMPLE_RATE_192KHZ,
+	AUDIO_SAMPLE_RATE_MAX
+};
+
 /* parameters for clock regeneration */
 struct hdmi_tx_audio_acr {
 	u32 n;
@@ -53,7 +75,7 @@
 
 struct hdmi_tx_audio_acr_arry {
 	u32 pclk;
-	struct hdmi_tx_audio_acr lut[HDMI_SAMPLE_RATE_MAX];
+	struct hdmi_tx_audio_acr lut[AUDIO_SAMPLE_RATE_MAX];
 };
 
 static int hdmi_tx_sysfs_enable_hpd(struct hdmi_tx_ctrl *hdmi_ctrl, int on);
@@ -91,35 +113,46 @@
 	}
 } /* hdmi_pm_name */
 
-static u8 hdmi_tx_avi_iframe_lut[][16] = {
-/*	480p60	480i60	576p50	576i50	720p60	 720p50	1080p60	1080i60	1080p50
-	1080i50	1080p24	1080p30	1080p25	640x480p 480p60_16_9 576p50_4_3 */
-	{0x10,	0x10,	0x10,	0x10,	0x10,	 0x10,	0x10,	0x10,	0x10,
-	 0x10,	0x10,	0x10,	0x10,	0x10, 0x10, 0x10}, /*00*/
-	{0x18,	0x18,	0x28,	0x28,	0x28,	 0x28,	0x28,	0x28,	0x28,
-	 0x28,	0x28,	0x28,	0x28,	0x18, 0x28, 0x18}, /*01*/
-	{0x00,	0x04,	0x04,	0x04,	0x04,	 0x04,	0x04,	0x04,	0x04,
-	 0x04,	0x04,	0x04,	0x04,	0x88, 0x00, 0x04}, /*02*/
-	{0x02,	0x06,	0x11,	0x15,	0x04,	 0x13,	0x10,	0x05,	0x1F,
-	 0x14,	0x20,	0x22,	0x21,	0x01, 0x03, 0x11}, /*03*/
-	{0x00,	0x01,	0x00,	0x01,	0x00,	 0x00,	0x00,	0x00,	0x00,
-	 0x00,	0x00,	0x00,	0x00,	0x00, 0x00, 0x00}, /*04*/
-	{0x00,	0x00,	0x00,	0x00,	0x00,	 0x00,	0x00,	0x00,	0x00,
-	 0x00,	0x00,	0x00,	0x00,	0x00, 0x00, 0x00}, /*05*/
-	{0x00,	0x00,	0x00,	0x00,	0x00,	 0x00,	0x00,	0x00,	0x00,
-	 0x00,	0x00,	0x00,	0x00,	0x00, 0x00, 0x00}, /*06*/
-	{0xE1,	0xE1,	0x41,	0x41,	0xD1,	 0xd1,	0x39,	0x39,	0x39,
-	 0x39,	0x39,	0x39,	0x39,	0xe1, 0xE1, 0x41}, /*07*/
-	{0x01,	0x01,	0x02,	0x02,	0x02,	 0x02,	0x04,	0x04,	0x04,
-	 0x04,	0x04,	0x04,	0x04,	0x01, 0x01, 0x02}, /*08*/
-	{0x00,	0x00,	0x00,	0x00,	0x00,	 0x00,	0x00,	0x00,	0x00,
-	 0x00,	0x00,	0x00,	0x00,	0x00, 0x00, 0x00}, /*09*/
-	{0x00,	0x00,	0x00,	0x00,	0x00,	 0x00,	0x00,	0x00,	0x00,
-	 0x00,	0x00,	0x00,	0x00,	0x00, 0x00, 0x00}, /*10*/
-	{0xD1,	0xD1,	0xD1,	0xD1,	0x01,	 0x01,	0x81,	0x81,	0x81,
-	 0x81,	0x81,	0x81,	0x81,	0x81, 0xD1, 0xD1}, /*11*/
-	{0x02,	0x02,	0x02,	0x02,	0x05,	 0x05,	0x07,	0x07,	0x07,
-	 0x07,	0x07,	0x07,	0x07,	0x02, 0x02, 0x02}  /*12*/
+static u8 hdmi_tx_avi_iframe_lut[][20] = {
+	{0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,
+	 0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,	0x10,
+	 0x10,	0x10}, /*00*/
+	{0x18,	0x18,	0x28,	0x28,	0x28,	0x28,	0x28,	0x28,	0x28,
+	 0x28,	0x28,	0x28,	0x28,	0x18,	0x28,	0x18,	0x28,	0x28,
+	 0x28,	0x28}, /*01*/
+	{0x00,	0x04,	0x04,	0x04,	0x04,	0x04,	0x04,	0x04,	0x04,
+	 0x04,	0x04,	0x04,	0x04,	0x88,	0x00,	0x04,	0x04,	0x04,
+	 0x04,	0x04}, /*02*/
+	{0x02,	0x06,	0x11,	0x15,	0x04,	0x13,	0x10,	0x05,	0x1F,
+	 0x14,	0x20,	0x22,	0x21,	0x01,	0x03,	0x11,	0x00,	0x00,
+	 0x00,	0x00}, /*03*/
+	{0x00,	0x01,	0x00,	0x01,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00}, /*04*/
+	{0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00}, /*05*/
+	{0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00}, /*06*/
+	{0xE1,	0xE1,	0x41,	0x41,	0xD1,	0xd1,	0x39,	0x39,	0x39,
+	 0x39,	0x39,	0x39,	0x39,	0xe1,	0xE1,	0x41,	0x71,	0x71,
+	 0x71,	0x71}, /*07*/
+	{0x01,	0x01,	0x02,	0x02,	0x02,	0x02,	0x04,	0x04,	0x04,
+	 0x04,	0x04,	0x04,	0x04,	0x01,	0x01,	0x02,	0x08,	0x08,
+	 0x08,	0x08}, /*08*/
+	{0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00}, /*09*/
+	{0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,	0x00,
+	 0x00,	0x00}, /*10*/
+	{0xD1,	0xD1,	0xD1,	0xD1,	0x01,	0x01,	0x81,	0x81,	0x81,
+	 0x81,	0x81,	0x81,	0x81,	0x81,	0xD1,	0xD1,	0x01,	0x01,
+	 0x01,	0x01}, /*11*/
+	{0x02,	0x02,	0x02,	0x02,	0x05,	0x05,	0x07,	0x07,	0x07,
+	 0x07,	0x07,	0x07,	0x07,	0x02,	0x02,	0x02,	0x0F,	0x0F,
+	 0x0F,	0x10}  /*12*/
 };
 
 /* Audio constants lookup table for hdmi_tx_audio_acr_setup */
@@ -141,6 +174,10 @@
 	{148500, {{4096, 148500}, {6272, 165000}, {6144, 148500},
 		{12544, 165000}, {12288, 148500}, {25088, 165000},
 		{24576, 148500} } },
+	/* 297.000MHz */
+	{297000, {{3072, 222750}, {4704, 247500}, {5120, 247500},
+		{9408, 247500}, {10240, 247500}, {18816, 247500},
+		{20480, 247500} } },
 };
 
 const char *hdmi_tx_pm_name(enum hdmi_tx_power_module_type module)
@@ -406,10 +443,75 @@
 	hdmi_ctrl->kobj = NULL;
 } /* hdmi_tx_sysfs_remove */
 
+static inline u32 hdmi_tx_is_dvi_mode(struct hdmi_tx_ctrl *hdmi_ctrl)
+{
+	return hdmi_edid_get_sink_mode(
+		hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID]) ? 0 : 1;
+} /* hdmi_tx_is_dvi_mode */
+
+static inline void hdmi_tx_set_audio_switch_node(struct hdmi_tx_ctrl *hdmi_ctrl,
+	int val, bool force)
+{
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	if (!hdmi_tx_is_dvi_mode(hdmi_ctrl) &&
+		(force || (hdmi_ctrl->audio_sdev.state != val))) {
+		switch_set_state(&hdmi_ctrl->audio_sdev, val);
+		DEV_INFO("%s: hdmi_audio state switched to %d\n", __func__,
+			hdmi_ctrl->audio_sdev.state);
+	}
+} /* hdmi_tx_set_audio_switch_node */
+
+void hdmi_tx_hdcp_cb(void *ptr, enum hdmi_hdcp_state status)
+{
+	int rc = 0;
+	struct hdmi_tx_ctrl *hdmi_ctrl = (struct hdmi_tx_ctrl *)ptr;
+
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	DEV_DBG("%s: HDCP status=%s hpd_state=%d\n", __func__,
+		hdcp_state_name(status), hdmi_ctrl->hpd_state);
+
+	switch (status) {
+	case HDCP_STATE_AUTHENTICATED:
+		if (hdmi_ctrl->hpd_state)
+			hdmi_tx_set_audio_switch_node(hdmi_ctrl, 1, false);
+		break;
+	case HDCP_STATE_AUTH_FAIL:
+		hdmi_tx_set_audio_switch_node(hdmi_ctrl, 0, false);
+
+		if (hdmi_ctrl->hpd_state) {
+			DEV_DBG("%s: Reauthenticating\n", __func__);
+			rc = hdmi_hdcp_reauthenticate(
+				hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]);
+			if (rc)
+				DEV_ERR("%s: HDCP reauth failed. rc=%d\n",
+					__func__, rc);
+		} else {
+			DEV_DBG("%s: Not reauthenticating. Cable not conn\n",
+				__func__);
+		}
+
+		break;
+	case HDCP_STATE_AUTHENTICATING:
+	case HDCP_STATE_INACTIVE:
+	default:
+		break;
+		/* do nothing */
+	}
+}
+
 /* Enable HDMI features */
 static int hdmi_tx_init_features(struct hdmi_tx_ctrl *hdmi_ctrl)
 {
 	struct hdmi_edid_init_data edid_init_data;
+	struct hdmi_hdcp_init_data hdcp_init_data;
 
 	if (!hdmi_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
@@ -431,6 +533,29 @@
 		hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID],
 		hdmi_ctrl->video_resolution);
 
+	/* Initialize HDCP feature */
+	if (hdmi_ctrl->present_hdcp) {
+		hdcp_init_data.core_io = &hdmi_ctrl->pdata.io[HDMI_TX_CORE_IO];
+		hdcp_init_data.qfprom_io =
+			&hdmi_ctrl->pdata.io[HDMI_TX_QFPROM_IO];
+		hdcp_init_data.mutex = &hdmi_ctrl->mutex;
+		hdcp_init_data.ddc_ctrl = &hdmi_ctrl->ddc_ctrl;
+		hdcp_init_data.workq = hdmi_ctrl->workq;
+		hdcp_init_data.notify_status = hdmi_tx_hdcp_cb;
+		hdcp_init_data.cb_data = (void *)hdmi_ctrl;
+
+		hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP] =
+			hdmi_hdcp_init(&hdcp_init_data);
+		if (!hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]) {
+			DEV_ERR("%s: hdmi_hdcp_init failed\n", __func__);
+			hdmi_edid_deinit(hdmi_ctrl->feature_data[
+				HDMI_TX_FEAT_EDID]);
+			hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID] = NULL;
+			return -EPERM;
+		}
+
+		DEV_DBG("%s: HDCP feature initialized\n", __func__);
+	}
 	return 0;
 } /* hdmi_tx_init_features */
 
@@ -440,12 +565,6 @@
 	return DSS_REG_R_ND(io, HDMI_CTRL) & BIT(0);
 } /* hdmi_tx_is_controller_on */
 
-static inline u32 hdmi_tx_is_dvi_mode(struct hdmi_tx_ctrl *hdmi_ctrl)
-{
-	return hdmi_edid_get_sink_mode(
-		hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID]) ? 0 : 1;
-} /* hdmi_tx_is_dvi_mode */
-
 static int hdmi_tx_init_panel_info(uint32_t resolution,
 	struct mdss_panel_info *pinfo)
 {
@@ -482,9 +601,11 @@
 } /* hdmi_tx_init_panel_info */
 
 /* Table indicating the video format supported by the HDMI TX Core */
-/* Valid Pixel-Clock rates: 25.2MHz, 27MHz, 27.03MHz, 74.25MHz, 148.5MHz */
+/* Valid pclk rates (Mhz): 25.2, 27, 27.03, 74.25, 148.5, 268.5, 297 */
 static void hdmi_tx_setup_video_mode_lut(void)
 {
+	hdmi_init_supported_video_timings();
+
 	hdmi_set_supported_mode(HDMI_VFRMT_640x480p60_4_3);
 	hdmi_set_supported_mode(HDMI_VFRMT_720x480p60_4_3);
 	hdmi_set_supported_mode(HDMI_VFRMT_720x480p60_16_9);
@@ -502,6 +623,11 @@
 	hdmi_set_supported_mode(HDMI_VFRMT_1920x1080p50_16_9);
 	hdmi_set_supported_mode(HDMI_VFRMT_1920x1080i60_16_9);
 	hdmi_set_supported_mode(HDMI_VFRMT_1920x1080p60_16_9);
+	hdmi_set_supported_mode(HDMI_VFRMT_2560x1600p60_16_9);
+	hdmi_set_supported_mode(HDMI_VFRMT_3840x2160p30_16_9);
+	hdmi_set_supported_mode(HDMI_VFRMT_3840x2160p25_16_9);
+	hdmi_set_supported_mode(HDMI_VFRMT_3840x2160p24_16_9);
+	hdmi_set_supported_mode(HDMI_VFRMT_4096x2160p24_16_9);
 } /* hdmi_tx_setup_video_mode_lut */
 
 static int hdmi_tx_read_sink_info(struct hdmi_tx_ctrl *hdmi_ctrl)
@@ -534,7 +660,6 @@
 static void hdmi_tx_hpd_int_work(struct work_struct *work)
 {
 	struct hdmi_tx_ctrl *hdmi_ctrl = NULL;
-	struct dss_io_data *io = NULL;
 
 	hdmi_ctrl = container_of(work, struct hdmi_tx_ctrl, hpd_int_work);
 	if (!hdmi_ctrl || !hdmi_ctrl->hpd_initialized) {
@@ -542,16 +667,8 @@
 		return;
 	}
 
-	io = &hdmi_ctrl->pdata.io[HDMI_TX_CORE_IO];
-	if (!io->base) {
-		DEV_ERR("%s: Core io is not initialized\n", __func__);
-		return;
-	}
-
 	DEV_DBG("%s: Got HPD interrupt\n", __func__);
 
-	hdmi_ctrl->hpd_state =
-		(DSS_REG_R(io, HDMI_HPD_INT_STATUS) & BIT(1)) >> 1;
 	if (hdmi_ctrl->hpd_state) {
 		hdmi_tx_read_sink_info(hdmi_ctrl);
 		DEV_INFO("HDMI HPD: sense CONNECTED: send ONLINE\n");
@@ -571,15 +688,22 @@
 		complete_all(&hdmi_ctrl->hpd_done);
 } /* hdmi_tx_hpd_int_work */
 
-static int hdmi_tx_check_capability(struct dss_io_data *io)
+static int hdmi_tx_check_capability(struct hdmi_tx_ctrl *hdmi_ctrl)
 {
 	u32 hdmi_disabled, hdcp_disabled;
+	struct dss_io_data *io = NULL;
 
-	if (!io) {
+	if (!hdmi_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
 		return -EINVAL;
 	}
 
+	io = &hdmi_ctrl->pdata.io[HDMI_TX_QFPROM_IO];
+	if (!io->base) {
+		DEV_ERR("%s: QFPROM io is not initialized\n", __func__);
+		return -EINVAL;
+	}
+
 	hdcp_disabled = DSS_REG_R_ND(io,
 		QFPROM_RAW_FEAT_CONFIG_ROW0_LSB) & BIT(31);
 
@@ -594,8 +718,13 @@
 		return -ENODEV;
 	}
 
-	if (hdcp_disabled)
+	if (hdcp_disabled) {
+		hdmi_ctrl->present_hdcp = 0;
 		DEV_WARN("%s: HDCP disabled\n", __func__);
+	} else {
+		hdmi_ctrl->present_hdcp = 1;
+		DEV_DBG("%s: Device is HDCP enabled\n", __func__);
+	}
 
 	return 0;
 } /* hdmi_tx_check_capability */
@@ -636,7 +765,7 @@
 	return 0;
 } /* hdmi_tx_set_video_fmt */
 
-static void hdmi_tx_video_setup(struct hdmi_tx_ctrl *hdmi_ctrl,
+static int hdmi_tx_video_setup(struct hdmi_tx_ctrl *hdmi_ctrl,
 	int video_format)
 {
 	u32 total_v   = 0;
@@ -652,47 +781,53 @@
 	if (timing == NULL) {
 		DEV_ERR("%s: video format not supported: %d\n", __func__,
 			video_format);
-		return;
+		return -EPERM;
 	}
 
 	if (!hdmi_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
-		return;
+		return -EINVAL;
 	}
 
 	io = &hdmi_ctrl->pdata.io[HDMI_TX_CORE_IO];
 	if (!io->base) {
 		DEV_ERR("%s: Core io is not initialized\n", __func__);
-		return;
+		return -EPERM;
 	}
 
 	total_h = timing->active_h + timing->front_porch_h +
 		timing->back_porch_h + timing->pulse_width_h - 1;
 	total_v = timing->active_v + timing->front_porch_v +
 		timing->back_porch_v + timing->pulse_width_v - 1;
-	DSS_REG_W(io, HDMI_TOTAL,
-		((total_v << 16) & 0x0FFF0000) |
-		((total_h << 0) & 0x00000FFF));
+	if (((total_v << 16) & 0xE0000000) || (total_h & 0xFFFFE000)) {
+		DEV_ERR("%s: total v=%d or h=%d is larger than supported\n",
+			__func__, total_v, total_h);
+		return -EPERM;
+	}
+	DSS_REG_W(io, HDMI_TOTAL, (total_v << 16) | (total_h << 0));
 
 	start_h = timing->back_porch_h + timing->pulse_width_h;
 	end_h   = (total_h + 1) - timing->front_porch_h;
-	DSS_REG_W(io, HDMI_ACTIVE_H,
-		((end_h << 16) & 0x0FFF0000) |
-		((start_h << 0) & 0x00000FFF));
+	if (((end_h << 16) & 0xE0000000) || (start_h & 0xFFFFE000)) {
+		DEV_ERR("%s: end_h=%d or start_h=%d is larger than supported\n",
+			__func__, end_h, start_h);
+		return -EPERM;
+	}
+	DSS_REG_W(io, HDMI_ACTIVE_H, (end_h << 16) | (start_h << 0));
 
 	start_v = timing->back_porch_v + timing->pulse_width_v - 1;
 	end_v   = total_v - timing->front_porch_v;
-	DSS_REG_W(io, HDMI_ACTIVE_V,
-		((end_v << 16) & 0x0FFF0000) |
-		((start_v << 0) & 0x00000FFF));
+	if (((end_v << 16) & 0xE0000000) || (start_v & 0xFFFFE000)) {
+		DEV_ERR("%s: end_v=%d or start_v=%d is larger than supported\n",
+			__func__, end_v, start_v);
+		return -EPERM;
+	}
+	DSS_REG_W(io, HDMI_ACTIVE_V, (end_v << 16) | (start_v << 0));
 
 	if (timing->interlaced) {
-		DSS_REG_W(io, HDMI_V_TOTAL_F2,
-			((total_v + 1) << 0) & 0x00000FFF);
-
+		DSS_REG_W(io, HDMI_V_TOTAL_F2, (total_v + 1) << 0);
 		DSS_REG_W(io, HDMI_ACTIVE_V_F2,
-			(((start_v + 1) << 0) & 0x00000FFF) |
-			(((end_v + 1) << 16) & 0x0FFF0000));
+			((end_v + 1) << 16) | ((start_v + 1) << 0));
 	} else {
 		DSS_REG_W(io, HDMI_V_TOTAL_F2, 0);
 		DSS_REG_W(io, HDMI_ACTIVE_V_F2, 0);
@@ -702,6 +837,8 @@
 		((timing->interlaced << 31) & 0x80000000) |
 		((timing->active_low_h << 29) & 0x20000000) |
 		((timing->active_low_v << 28) & 0x10000000));
+
+	return 0;
 } /* hdmi_tx_video_setup */
 
 static void hdmi_tx_set_avi_infoframe(struct hdmi_tx_ctrl *hdmi_ctrl)
@@ -772,6 +909,18 @@
 	case HDMI_VFRMT_720x576p50_4_3:
 		mode = 15;
 		break;
+	case HDMI_VFRMT_3840x2160p30_16_9:
+		mode = 16;
+		break;
+	case HDMI_VFRMT_3840x2160p25_16_9:
+		mode = 17;
+		break;
+	case HDMI_VFRMT_3840x2160p24_16_9:
+		mode = 18;
+		break;
+	case HDMI_VFRMT_4096x2160p24_16_9:
+		mode = 19;
+		break;
 	default:
 		DEV_INFO("%s: mode %d not supported\n", __func__,
 			hdmi_ctrl->video_resolution);
@@ -846,12 +995,87 @@
 	regVal = regVal << 8 | avi_iframe[14];
 	DSS_REG_W(io, HDMI_AVI_INFO3, regVal);
 
-	/* 0x3 for AVI InfFrame enable (every frame) */
+	/* AVI InfFrame enable (every frame) */
 	DSS_REG_W(io, HDMI_INFOFRAME_CTRL0,
-		DSS_REG_R(io, HDMI_INFOFRAME_CTRL0) |
-		0x00000003L);
+		DSS_REG_R(io, HDMI_INFOFRAME_CTRL0) | BIT(1) | BIT(0));
 } /* hdmi_tx_set_avi_infoframe */
 
+/* todo: add 3D support */
+static void hdmi_tx_set_vendor_specific_infoframe(
+	struct hdmi_tx_ctrl *hdmi_ctrl)
+{
+	int i;
+	u8 vs_iframe[9]; /* two header + length + 6 data */
+	u32 sum, reg_val;
+	u32 hdmi_vic, hdmi_video_format;
+	struct dss_io_data *io = NULL;
+
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return;
+	}
+
+	io = &hdmi_ctrl->pdata.io[HDMI_TX_CORE_IO];
+	if (!io->base) {
+		DEV_ERR("%s: Core io is not initialized\n", __func__);
+		return;
+	}
+
+	/* HDMI Spec 1.4a Table 8-10 */
+	vs_iframe[0] = 0x81; /* type */
+	vs_iframe[1] = 0x1;  /* version */
+	vs_iframe[2] = 0x8;  /* length */
+
+	vs_iframe[3] = 0x0; /* PB0: checksum */
+
+	/* PB1..PB3: 24 Bit IEEE Registration Code 00_0C_03 */
+	vs_iframe[4] = 0x03;
+	vs_iframe[5] = 0x0C;
+	vs_iframe[6] = 0x00;
+
+	hdmi_video_format = 0x1;
+	switch (hdmi_ctrl->video_resolution) {
+	case HDMI_VFRMT_3840x2160p30_16_9:
+		hdmi_vic = 0x1;
+		break;
+	case HDMI_VFRMT_3840x2160p25_16_9:
+		hdmi_vic = 0x2;
+		break;
+	case HDMI_VFRMT_3840x2160p24_16_9:
+		hdmi_vic = 0x3;
+		break;
+	case HDMI_VFRMT_4096x2160p24_16_9:
+		hdmi_vic = 0x4;
+		break;
+	default:
+		hdmi_video_format = 0x0;
+		hdmi_vic = 0x0;
+	}
+
+	/* PB4: HDMI Video Format[7:5],  Reserved[4:0] */
+	vs_iframe[7] = (hdmi_video_format << 5) & 0xE0;
+
+	/* PB5: HDMI_VIC or 3D_Structure[7:4], Reserved[3:0] */
+	vs_iframe[8] = hdmi_vic;
+
+	/* compute checksum */
+	sum = 0;
+	for (i = 0; i < 9; i++)
+		sum += vs_iframe[i];
+
+	sum &= 0xFF;
+	sum = 256 - sum;
+	vs_iframe[3] = (u8)sum;
+
+	reg_val = (hdmi_vic << 16) | (vs_iframe[3] << 8) |
+		(hdmi_video_format << 5) | vs_iframe[2];
+	DSS_REG_W(io, HDMI_VENSPEC_INFO0, reg_val);
+
+	/* vendor specific info-frame enable (every frame) */
+	DSS_REG_W(io, HDMI_INFOFRAME_CTRL0,
+		DSS_REG_R(io, HDMI_INFOFRAME_CTRL0) | BIT(13) | BIT(12));
+} /* hdmi_tx_set_vendor_specific_infoframe */
+
 static void hdmi_tx_set_spd_infoframe(struct hdmi_tx_ctrl *hdmi_ctrl)
 {
 	u32 packet_header  = 0;
@@ -961,8 +1185,9 @@
 
 static void hdmi_tx_set_mode(struct hdmi_tx_ctrl *hdmi_ctrl, u32 power_on)
 {
-	u32 reg_val = 0;
 	struct dss_io_data *io = NULL;
+	/* Defaults: Disable block, HDMI mode */
+	u32 reg_val = BIT(1);
 
 	if (!hdmi_ctrl) {
 		DEV_ERR("%s: invalid input\n", __func__);
@@ -975,23 +1200,17 @@
 	}
 
 	if (power_on) {
-		/* ENABLE */
-		reg_val |= BIT(0); /* Enable the block */
+		/* Enable the block */
+		reg_val |= BIT(0);
+
+		/* HDMI Encryption, if HDCP is enabled */
+		if (hdmi_ctrl->hdcp_feature_on && hdmi_ctrl->present_hdcp)
+			reg_val |= BIT(2);
+
+		/* Set transmission mode to DVI based in EDID info */
 		if (hdmi_edid_get_sink_mode(
-			hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID]) == 0) {
-			if (hdmi_ctrl->present_hdcp)
-				/* HDMI Encryption */
-				reg_val |= BIT(2);
-			reg_val |= BIT(1);
-		} else {
-			if (hdmi_ctrl->present_hdcp)
-				/* HDMI_Encryption_ON */
-				reg_val |= BIT(1) | BIT(2);
-			else
-				reg_val |= BIT(1);
-		}
-	} else {
-		reg_val = BIT(1);
+			hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID]) == 0)
+			reg_val &= ~BIT(1); /* DVI mode */
 	}
 
 	DSS_REG_W(io, HDMI_CTRL, reg_val);
@@ -1300,13 +1519,13 @@
 		layout = (MSM_HDMI_AUDIO_CHANNEL_2 == num_of_channels) ? 0 : 1;
 
 		if (
-		(HDMI_SAMPLE_RATE_192KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_176_4KHZ == hdmi_ctrl->audio_sample_rate)) {
+		(AUDIO_SAMPLE_RATE_192KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_176_4KHZ == hdmi_ctrl->audio_sample_rate)) {
 			multiplier = 4;
 			n >>= 2; /* divide N by 4 and use multiplier */
 		} else if (
-		(HDMI_SAMPLE_RATE_96KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_88_2KHZ == hdmi_ctrl->audio_sample_rate)) {
+		(AUDIO_SAMPLE_RATE_96KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_88_2KHZ == hdmi_ctrl->audio_sample_rate)) {
 			multiplier = 2;
 			n >>= 1; /* divide N by 2 and use multiplier */
 		} else {
@@ -1320,9 +1539,9 @@
 		/* N_MULTIPLE(multiplier) */
 		acr_pck_ctrl_reg |= (multiplier & 7) << 16;
 
-		if ((HDMI_SAMPLE_RATE_48KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_96KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_192KHZ == hdmi_ctrl->audio_sample_rate)) {
+		if ((AUDIO_SAMPLE_RATE_48KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_96KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_192KHZ == hdmi_ctrl->audio_sample_rate)) {
 			/* SELECT(3) */
 			acr_pck_ctrl_reg |= 3 << 4;
 			/* CTS_48 */
@@ -1333,9 +1552,9 @@
 			/* N */
 			DSS_REG_W(io, HDMI_ACR_48_1, n);
 		} else if (
-		(HDMI_SAMPLE_RATE_44_1KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_88_2KHZ == hdmi_ctrl->audio_sample_rate) ||
-		(HDMI_SAMPLE_RATE_176_4KHZ == hdmi_ctrl->audio_sample_rate)) {
+		(AUDIO_SAMPLE_RATE_44_1KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_88_2KHZ == hdmi_ctrl->audio_sample_rate) ||
+		(AUDIO_SAMPLE_RATE_176_4KHZ == hdmi_ctrl->audio_sample_rate)) {
 			/* SELECT(2) */
 			acr_pck_ctrl_reg |= 2 << 4;
 			/* CTS_44 */
@@ -1373,11 +1592,10 @@
 	return 0;
 } /* hdmi_tx_audio_acr_setup */
 
-static int hdmi_tx_audio_info_setup(void *priv_d, bool enabled,
-	u32 num_of_channels, u32 channel_allocation, u32 level_shift,
-	bool down_mix)
+static int hdmi_tx_audio_iframe_setup(struct hdmi_tx_ctrl *hdmi_ctrl,
+	bool enabled, u32 num_of_channels, u32 channel_allocation,
+	u32 level_shift, bool down_mix)
 {
-	struct hdmi_tx_ctrl *hdmi_ctrl = (struct hdmi_tx_ctrl *)priv_d;
 	struct dss_io_data *io = NULL;
 
 	u32 channel_count = 1; /* Def to 2 channels -> Table 17 in CEA-D */
@@ -1422,22 +1640,15 @@
 	if (enabled) {
 		switch (num_of_channels) {
 		case MSM_HDMI_AUDIO_CHANNEL_2:
-			channel_allocation = 0;	/* Default to FR, FL */
 			break;
 		case MSM_HDMI_AUDIO_CHANNEL_4:
 			channel_count = 3;
-			/* FC, LFE, FR, FL */
-			channel_allocation = 0x3;
 			break;
 		case MSM_HDMI_AUDIO_CHANNEL_6:
 			channel_count = 5;
-			/* RR, RL, FC, LFE, FR, FL */
-			channel_allocation = 0xB;
 			break;
 		case MSM_HDMI_AUDIO_CHANNEL_8:
 			channel_count = 7;
-			/* FRC, FLC, RR, RL, FC, LFE, FR, FL */
-			channel_allocation = 0x1f;
 			break;
 		default:
 			DEV_ERR("%s: Unsupported num_of_channels = %u\n",
@@ -1505,8 +1716,66 @@
 		enabled ? "HDMI-AUDIO-ON: " : "HDMI-AUDIO-OFF: ", REG_DUMP);
 
 	return 0;
+} /* hdmi_tx_audio_iframe_setup */
+
+static int hdmi_tx_audio_info_setup(struct platform_device *pdev,
+	u32 num_of_channels, u32 channel_allocation, u32 level_shift,
+	bool down_mix)
+{
+	int rc = 0;
+	struct hdmi_tx_ctrl *hdmi_ctrl = platform_get_drvdata(pdev);
+
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -ENODEV;
+	}
+
+	if (hdmi_ctrl->panel_power_on) {
+		rc = hdmi_tx_audio_iframe_setup(hdmi_ctrl, true,
+			num_of_channels, channel_allocation, level_shift,
+			down_mix);
+		if (rc)
+			DEV_ERR("%s: hdmi_tx_audio_iframe_setup failed.rc=%d\n",
+				__func__, rc);
+	} else {
+		DEV_ERR("%s: Error. panel is not on.\n", __func__);
+		rc = -EPERM;
+	}
+
+	return 0;
 } /* hdmi_tx_audio_info_setup */
 
+static int hdmi_tx_get_audio_edid_blk(struct platform_device *pdev,
+	struct msm_hdmi_audio_edid_blk *blk)
+{
+	struct hdmi_tx_ctrl *hdmi_ctrl = platform_get_drvdata(pdev);
+
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -ENODEV;
+	}
+
+	return hdmi_edid_get_audio_blk(
+		hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID], blk);
+} /* hdmi_tx_get_audio_edid_blk */
+
+int msm_hdmi_register_audio_codec(struct platform_device *pdev,
+	struct msm_hdmi_audio_codec_ops *ops)
+{
+	struct hdmi_tx_ctrl *hdmi_ctrl = platform_get_drvdata(pdev);
+
+	if (!hdmi_ctrl) {
+		DEV_ERR("%s: invalid input\n", __func__);
+		return -ENODEV;
+	}
+
+	ops->audio_info_setup = hdmi_tx_audio_info_setup;
+	ops->get_audio_edid_blk = hdmi_tx_get_audio_edid_blk;
+
+	return 0;
+} /* hdmi_tx_audio_register */
+EXPORT_SYMBOL(msm_hdmi_register_audio_codec);
+
 static int hdmi_tx_audio_setup(struct hdmi_tx_ctrl *hdmi_ctrl)
 {
 	int rc = 0;
@@ -1531,9 +1800,9 @@
 		return rc;
 	}
 
-	rc = hdmi_tx_audio_info_setup(hdmi_ctrl, true, channels, 0, 0, false);
+	rc = hdmi_tx_audio_iframe_setup(hdmi_ctrl, true, channels, 0, 0, false);
 	if (rc) {
-		DEV_ERR("%s: hdmi_tx_audio_info_setup failed. rc=%d\n",
+		DEV_ERR("%s: hdmi_tx_audio_iframe_setup failed. rc=%d\n",
 			__func__, rc);
 		return rc;
 	}
@@ -1545,7 +1814,7 @@
 
 static void hdmi_tx_audio_off(struct hdmi_tx_ctrl *hdmi_ctrl)
 {
-	u32 i, status, max_reads, timeout_us, timeout_sec = 15;
+	u32 i, status, sleep_us, timeout_us, timeout_sec = 15;
 	struct dss_io_data *io = NULL;
 
 	if (!hdmi_ctrl) {
@@ -1562,17 +1831,17 @@
 	/* Check if audio engine is turned off by QDSP or not */
 	/* send off notification after every 1 sec for 15 seconds */
 	for (i = 0; i < timeout_sec; i++) {
-		max_reads = 500;
-		timeout_us = 1000 * 2;
+		sleep_us = 5000; /* Maximum time to sleep between two reads */
+		timeout_us = 1000 * 1000; /* Total time for condition to meet */
 
-		if (readl_poll_timeout_noirq((io->base + HDMI_AUDIO_CFG),
+		if (readl_poll_timeout((io->base + HDMI_AUDIO_CFG),
 			status, ((status & BIT(0)) == 0),
-			max_reads, timeout_us)) {
+			sleep_us, timeout_us)) {
 
 			DEV_ERR("%s: audio still on after %d sec. try again\n",
 				__func__, i+1);
 
-			switch_set_state(&hdmi_ctrl->audio_sdev, 0);
+			hdmi_tx_set_audio_switch_node(hdmi_ctrl, 0, true);
 			continue;
 		}
 		break;
@@ -1580,8 +1849,8 @@
 	if (i == timeout_sec)
 		DEV_ERR("%s: Error: cannot turn off audio engine\n", __func__);
 
-	if (hdmi_tx_audio_info_setup(hdmi_ctrl, false, 0, 0, 0, false))
-		DEV_ERR("%s: hdmi_tx_audio_info_setup failed.\n", __func__);
+	if (hdmi_tx_audio_iframe_setup(hdmi_ctrl, false, 0, 0, 0, false))
+		DEV_ERR("%s: hdmi_tx_audio_iframe_setup failed.\n", __func__);
 
 	if (hdmi_tx_audio_acr_setup(hdmi_ctrl, false, 0))
 		DEV_ERR("%s: hdmi_tx_audio_acr_setup failed.\n", __func__);
@@ -1610,7 +1879,13 @@
 
 	hdmi_tx_set_mode(hdmi_ctrl, true);
 
-	hdmi_tx_video_setup(hdmi_ctrl, hdmi_ctrl->video_resolution);
+	rc = hdmi_tx_video_setup(hdmi_ctrl, hdmi_ctrl->video_resolution);
+	if (rc) {
+		DEV_ERR("%s: hdmi_tx_video_setup failed. rc=%d\n",
+			__func__, rc);
+		hdmi_tx_set_mode(hdmi_ctrl, false);
+		return rc;
+	}
 
 	if (!hdmi_tx_is_dvi_mode(hdmi_ctrl)) {
 		rc = hdmi_tx_audio_setup(hdmi_ctrl);
@@ -1621,16 +1896,15 @@
 			return rc;
 		}
 
-		switch_set_state(&hdmi_ctrl->audio_sdev, 1);
-		DEV_INFO("%s: hdmi_audio state switch to %d\n", __func__,
-			hdmi_ctrl->audio_sdev.state);
+		if (!hdmi_ctrl->hdcp_feature_on || !hdmi_ctrl->present_hdcp)
+			hdmi_tx_set_audio_switch_node(hdmi_ctrl, 1, false);
+
+		hdmi_tx_set_avi_infoframe(hdmi_ctrl);
+		hdmi_tx_set_vendor_specific_infoframe(hdmi_ctrl);
+		hdmi_tx_set_spd_infoframe(hdmi_ctrl);
 	}
 
-	hdmi_tx_set_avi_infoframe(hdmi_ctrl);
-	/* todo: CONFIG_FB_MSM_HDMI_3D */
-	hdmi_tx_set_spd_infoframe(hdmi_ctrl);
-
-	/* todo: HDCP/CEC */
+	/* todo: CEC */
 
 	DEV_INFO("%s: HDMI Core: Initialized\n", __func__);
 
@@ -1689,11 +1963,13 @@
 		return;
 	}
 
-	if (!hdmi_tx_is_dvi_mode(hdmi_ctrl)) {
-		switch_set_state(&hdmi_ctrl->audio_sdev, 0);
-		DEV_INFO("%s: hdmi_audio state switch to %d\n", __func__,
-			hdmi_ctrl->audio_sdev.state);
+	if (hdmi_ctrl->hdcp_feature_on && hdmi_ctrl->present_hdcp) {
+		DEV_DBG("%s: Turning off HDCP\n", __func__);
+		hdmi_hdcp_off(hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]);
+	}
 
+	if (!hdmi_tx_is_dvi_mode(hdmi_ctrl)) {
+		hdmi_tx_set_audio_switch_node(hdmi_ctrl, 0, false);
 		hdmi_tx_audio_off(hdmi_ctrl);
 	}
 
@@ -1772,6 +2048,8 @@
 		return rc;
 	}
 
+	hdmi_ctrl->hdcp_feature_on = hdcp_feature_on;
+
 	DEV_INFO("power: ON (%s)\n", hdmi_get_video_fmt_2string(
 		hdmi_ctrl->video_resolution));
 
@@ -1794,7 +2072,6 @@
 			hdmi_tx_power_off(panel_data);
 			return rc;
 		}
-		/* todo: HDCP */
 	} else {
 		mutex_unlock(&hdmi_ctrl->mutex);
 	}
@@ -1937,6 +2214,9 @@
 	}
 
 	if (DSS_REG_R(io, HDMI_HPD_INT_STATUS) & BIT(0)) {
+		hdmi_ctrl->hpd_state =
+			(DSS_REG_R(io, HDMI_HPD_INT_STATUS) & BIT(1)) >> 1;
+
 		/*
 		 * Ack the current hpd interrupt and stop listening to
 		 * new hpd interrupt.
@@ -1948,6 +2228,11 @@
 	if (hdmi_ddc_isr(&hdmi_ctrl->ddc_ctrl) < 0)
 		DEV_ERR("%s: hdmi_ddc_isr failed\n", __func__);
 
+	if (hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP])
+		if (hdmi_hdcp_isr(
+			hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]) < 0)
+			DEV_ERR("%s: hdmi_hdcp_isr failed\n", __func__);
+
 	return IRQ_HANDLED;
 } /* hdmi_tx_isr */
 
@@ -1958,6 +2243,11 @@
 		return;
 	}
 
+	if (hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]) {
+		hdmi_hdcp_deinit(hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]);
+		hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP] = NULL;
+	}
+
 	if (hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID])
 		hdmi_edid_deinit(hdmi_ctrl->feature_data[HDMI_TX_FEAT_EDID]);
 
@@ -1982,7 +2272,7 @@
 
 	pdata = &hdmi_ctrl->pdata;
 
-	rc = hdmi_tx_check_capability(&pdata->io[HDMI_TX_QFPROM_IO]);
+	rc = hdmi_tx_check_capability(hdmi_ctrl);
 	if (rc) {
 		DEV_ERR("%s: no HDMI device\n", __func__);
 		goto fail_no_hdmi;
@@ -2014,7 +2304,7 @@
 
 	INIT_WORK(&hdmi_ctrl->power_off_work, hdmi_tx_power_off_work);
 
-	hdmi_ctrl->audio_sample_rate = HDMI_SAMPLE_RATE_48KHZ;
+	hdmi_ctrl->audio_sample_rate = AUDIO_SAMPLE_RATE_48KHZ;
 
 	hdmi_ctrl->sdev.name = "hdmi";
 	if (switch_dev_register(&hdmi_ctrl->sdev) < 0) {
@@ -2119,6 +2409,14 @@
 		break;
 
 	case MDSS_EVENT_TIMEGEN_ON:
+		if (hdmi_ctrl->hdcp_feature_on && hdmi_ctrl->present_hdcp) {
+			DEV_DBG("%s: Starting HDCP authentication\n", __func__);
+			rc = hdmi_hdcp_authenticate(
+				hdmi_ctrl->feature_data[HDMI_TX_FEAT_HDCP]);
+			if (rc)
+				DEV_ERR("%s: hdcp auth failed. rc=%d\n",
+					__func__, rc);
+		}
 		break;
 
 	case MDSS_EVENT_SUSPEND:
@@ -2146,9 +2444,6 @@
 		break;
 
 	case MDSS_EVENT_TIMEGEN_OFF:
-		/* If a power off is already underway, wait for it to finish */
-		if (hdmi_ctrl->panel_suspend)
-			flush_work_sync(&hdmi_ctrl->power_off_work);
 		break;
 
 	case MDSS_EVENT_CLOSE:
@@ -2792,6 +3087,15 @@
 		goto failed_init_features;
 	}
 
+	rc = of_platform_populate(of_node, NULL, NULL, &pdev->dev);
+	if (rc) {
+		DEV_ERR("%s: failed to add child devices, rc=%d\n",
+			__func__, rc);
+		goto failed_init_features;
+	} else {
+		DEV_DBG("%s: added child devices.\n", __func__);
+	}
+
 	return rc;
 
 failed_init_features:
@@ -2855,6 +3159,26 @@
 	platform_driver_unregister(&this_driver);
 } /* hdmi_tx_drv_exit */
 
+static int set_hdcp_feature_on(const char *val, const struct kernel_param *kp)
+{
+	int rc = 0;
+
+	rc = param_set_bool(val, kp);
+	if (!rc)
+		pr_debug("%s: HDCP feature = %d\n", __func__, hdcp_feature_on);
+
+	return rc;
+}
+
+static struct kernel_param_ops hdcp_feature_on_param_ops = {
+	.set = set_hdcp_feature_on,
+	.get = param_get_bool,
+};
+
+module_param_cb(hdcp, &hdcp_feature_on_param_ops, &hdcp_feature_on,
+	S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(hdcp, "Enable or Disable HDCP");
+
 module_init(hdmi_tx_drv_init);
 module_exit(hdmi_tx_drv_exit);
 
diff --git a/drivers/video/msm/mdss/mdss_hdmi_tx.h b/drivers/video/msm/mdss/mdss_hdmi_tx.h
index 5f8094f..f78ce9f 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_tx.h
+++ b/drivers/video/msm/mdss/mdss_hdmi_tx.h
@@ -63,6 +63,7 @@
 
 	struct work_struct power_off_work;
 
+	bool hdcp_feature_on;
 	u32 present_hdcp;
 
 	u8 spd_vendor_name[8];
diff --git a/drivers/video/msm/mdss/mdss_hdmi_util.c b/drivers/video/msm/mdss/mdss_hdmi_util.c
index a3d76be..ad63605 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_util.c
+++ b/drivers/video/msm/mdss/mdss_hdmi_util.c
@@ -16,73 +16,24 @@
 #include "mdss_hdmi_util.h"
 
 static struct hdmi_disp_mode_timing_type
-	hdmi_supported_video_mode_lut[HDMI_VFRMT_MAX] = {
-	HDMI_SETTINGS_640x480p60_4_3,
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1280x720p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080i60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x240p60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x240p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x480i60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x480i60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x240p60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x240p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480p60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1280x720p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080i50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x288p50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x288p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x576i50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x576i50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x288p50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x288p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576p50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080p24_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080p25_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080p30_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x480p60_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x480p60_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x576p50_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_2880x576p50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1250i50_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080i100_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1280x720p100_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p100_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p100_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i100_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i100_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1920x1080i120_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1280x720p120_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p120_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p120_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i120_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i120_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p200_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x576p200_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i200_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x576i200_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p240_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_720x480p240_16_9),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i240_4_3),
-	VFRMT_NOT_SUPPORTED(HDMI_VFRMT_1440x480i240_16_9),
-}; /* hdmi_supported_video_mode_lut */
+	hdmi_supported_video_mode_lut[HDMI_VFRMT_MAX];
 
 #define HDMI_SETUP_LUT(MODE) do {					\
 	struct hdmi_disp_mode_timing_type mode = HDMI_SETTINGS_##MODE;	\
 	hdmi_supported_video_mode_lut[mode.video_format] = mode;	\
 	} while (0)
 
+void hdmi_init_supported_video_timings(void)
+{
+	int i;
+
+	for (i = 0; i < HDMI_VFRMT_MAX; i++) {
+		struct hdmi_disp_mode_timing_type mode = VFRMT_NOT_SUPPORTED(i);
+
+		hdmi_supported_video_mode_lut[i] = mode;
+	}
+} /* hdmi_init_supported_video_timings */
+
 const struct hdmi_disp_mode_timing_type *hdmi_get_supported_mode(u32 mode)
 {
 	const struct hdmi_disp_mode_timing_type *ret = NULL;
@@ -203,6 +154,21 @@
 	case HDMI_VFRMT_1920x1080p60_16_9:
 		HDMI_SETUP_LUT(1920x1080p60_16_9);
 		break;
+	case HDMI_VFRMT_2560x1600p60_16_9:
+		HDMI_SETUP_LUT(2560x1600p60_16_9);
+		break;
+	case HDMI_VFRMT_3840x2160p30_16_9:
+		HDMI_SETUP_LUT(3840x2160p30_16_9);
+		break;
+	case HDMI_VFRMT_3840x2160p25_16_9:
+		HDMI_SETUP_LUT(3840x2160p25_16_9);
+		break;
+	case HDMI_VFRMT_3840x2160p24_16_9:
+		HDMI_SETUP_LUT(3840x2160p24_16_9);
+		break;
+	case HDMI_VFRMT_4096x2160p24_16_9:
+		HDMI_SETUP_LUT(4096x2160p24_16_9);
+		break;
 	default:
 		DEV_ERR("%s: unsupported mode=%d\n", __func__, mode);
 	}
@@ -270,6 +236,11 @@
 	case HDMI_VFRMT_720x480p240_16_9:  return " 720x 480 p240 16/9";
 	case HDMI_VFRMT_1440x480i240_4_3:  return "1440x 480 i240  4/3";
 	case HDMI_VFRMT_1440x480i240_16_9: return "1440x 480 i240 16/9";
+	case HDMI_VFRMT_2560x1600p60_16_9: return "2560x1600 p60 16/9";
+	case HDMI_VFRMT_3840x2160p30_16_9: return "3840x2160 p30 16/9";
+	case HDMI_VFRMT_3840x2160p25_16_9: return "3840x2160 p25 16/9";
+	case HDMI_VFRMT_3840x2160p24_16_9: return "3840x2160 p24 16/9";
+	case HDMI_VFRMT_4096x2160p24_16_9: return "4096x2160 p24 16/9";
 	default:                           return "???";
 	}
 } /* hdmi_get_video_fmt_2string */
@@ -550,8 +521,8 @@
 	 */
 	DSS_REG_W_ND(ddc_ctrl->io, HDMI_DDC_SETUP, 0xFF000000);
 
-	/* Enable reference timer to 27 micro-seconds */
-	DSS_REG_W_ND(ddc_ctrl->io, HDMI_DDC_REF, (1 << 16) | (27 << 0));
+	/* Enable reference timer to 19 micro-seconds */
+	DSS_REG_W_ND(ddc_ctrl->io, HDMI_DDC_REF, (1 << 16) | (19 << 0));
 } /* hdmi_ddc_config */
 
 int hdmi_ddc_isr(struct hdmi_tx_ddc_ctrl *ddc_ctrl)
diff --git a/drivers/video/msm/mdss/mdss_hdmi_util.h b/drivers/video/msm/mdss/mdss_hdmi_util.h
index c970ebe..d79b6e7 100644
--- a/drivers/video/msm/mdss/mdss_hdmi_util.h
+++ b/drivers/video/msm/mdss/mdss_hdmi_util.h
@@ -215,8 +215,10 @@
 /* QFPROM Registers for HDMI/HDCP */
 #define QFPROM_RAW_FEAT_CONFIG_ROW0_LSB  (0x000000F8)
 #define QFPROM_RAW_FEAT_CONFIG_ROW0_MSB  (0x000000FC)
+#define HDCP_KSV_LSB                     (0x000060D8)
+#define HDCP_KSV_MSB                     (0x000060DC)
 
-/* all video formats defined by EIA CEA 861D */
+/* all video formats defined by EIA CEA-861-E */
 #define HDMI_VFRMT_640x480p60_4_3	0
 #define HDMI_VFRMT_720x480p60_4_3	1
 #define HDMI_VFRMT_720x480p60_16_9	2
@@ -292,7 +294,18 @@
 #define HDMI_VFRMT_1440x480i240_4_3	HDMI_VFRMT_720x480i240_4_3
 #define HDMI_VFRMT_720x480i240_16_9	58
 #define HDMI_VFRMT_1440x480i240_16_9	HDMI_VFRMT_720x480i240_16_9
-#define HDMI_VFRMT_MAX			59
+/* Video Identification Codes from 65-127 are reserved for the future */
+#define HDMI_VFRMT_END			127
+/* extended video formats */
+#define HDMI_VFRMT_3840x2160p30_16_9	(HDMI_VFRMT_END + 1)
+#define HDMI_VFRMT_3840x2160p25_16_9	(HDMI_VFRMT_END + 2)
+#define HDMI_VFRMT_3840x2160p24_16_9	(HDMI_VFRMT_END + 3)
+#define HDMI_VFRMT_4096x2160p24_16_9	(HDMI_VFRMT_END + 4)
+#define HDMI_EVFRMT_END			HDMI_VFRMT_4096x2160p24_16_9
+/* DVI only resolutions */
+#define HDMI_VFRMT_2560x1600p60_16_9	(HDMI_EVFRMT_END + 1)
+#define DVI_VFRMT_END			HDMI_VFRMT_2560x1600p60_16_9
+#define HDMI_VFRMT_MAX			(DVI_VFRMT_END + 1)
 #define HDMI_VFRMT_FORCE_32BIT		0x7FFFFFFF
 
 #define VFRMT_NOT_SUPPORTED(VFRMT) \
@@ -349,6 +362,21 @@
 #define HDMI_SETTINGS_1920x1080p30_16_9					\
 	{HDMI_VFRMT_1920x1080p30_16_9,   1920,  88,   44,  148,  false,	\
 	 1080, 4, 5, 36, false, 74250, 30000, false, true}
+#define HDMI_SETTINGS_2560x1600p60_16_9					\
+	{HDMI_VFRMT_2560x1600p60_16_9,   2560,  48,   32,  80,  false,	\
+	 1600, 3, 6, 37, false, 268500, 60000, false, true}
+#define HDMI_SETTINGS_3840x2160p30_16_9					\
+	{HDMI_VFRMT_3840x2160p30_16_9, 3840, 176, 88, 296, false,	\
+	 2160, 8, 10, 72, false, 297000, 30000, false, true}
+#define HDMI_SETTINGS_3840x2160p25_16_9					\
+	{HDMI_VFRMT_3840x2160p25_16_9, 3840, 1056, 88, 296, false,	\
+	 2160, 8, 10, 72, false, 297000, 25000, false, true}
+#define HDMI_SETTINGS_3840x2160p24_16_9					\
+	{HDMI_VFRMT_3840x2160p24_16_9, 3840, 1276, 88, 296, false,	\
+	 2160, 8, 10, 72, false, 297000, 24000, false, true}
+#define HDMI_SETTINGS_4096x2160p24_16_9					\
+	{HDMI_VFRMT_4096x2160p24_16_9, 4096, 1020, 88, 296, false,	\
+	 2160, 8, 10, 72, false, 297000, 24000, false, true}
 
 #define TOP_AND_BOTTOM		0x10
 #define FRAME_PACKING		0x20
@@ -397,6 +425,8 @@
 	int retry;
 };
 
+/* video timing related utility routines */
+void hdmi_init_supported_video_timings(void);
 int hdmi_get_video_id_code(struct hdmi_disp_mode_timing_type *timing_in);
 const struct hdmi_disp_mode_timing_type *hdmi_get_supported_mode(u32 mode);
 void hdmi_set_supported_mode(u32 mode);
diff --git a/drivers/video/msm/mdss/mdss_mdp.c b/drivers/video/msm/mdss/mdss_mdp.c
index b59d193..7ca06ff 100644
--- a/drivers/video/msm/mdss/mdss_mdp.c
+++ b/drivers/video/msm/mdss/mdss_mdp.c
@@ -666,7 +666,7 @@
 }
 
 static int mdss_iommu_fault_handler(struct iommu_domain *domain,
-		struct device *dev, unsigned long iova, int flags)
+		struct device *dev, unsigned long iova, int flags, void *token)
 {
 	pr_err("MDP IOMMU page fault: iova 0x%lx\n", iova);
 	return 0;
@@ -758,7 +758,7 @@
 				iomap->domain_idx);
 			return -EINVAL;
 		}
-		iommu_set_fault_handler(domain, mdss_iommu_fault_handler);
+		iommu_set_fault_handler(domain, mdss_iommu_fault_handler, NULL);
 
 		iomap->ctx = msm_iommu_get_ctx(iomap->ctx_name);
 		if (!iomap->ctx) {
diff --git a/drivers/video/msm/mdss/mdss_mdp_ctl.c b/drivers/video/msm/mdss/mdss_mdp_ctl.c
index 31cc527..bd0f6c9 100644
--- a/drivers/video/msm/mdss/mdss_mdp_ctl.c
+++ b/drivers/video/msm/mdss/mdss_mdp_ctl.c
@@ -94,6 +94,7 @@
 	u32 quota, rate;
 	u32 v_total, v_active;
 	int i;
+	u32 max_clk_rate = 0, ab_total = 0, ib_total = 0;
 
 	*bus_ab_quota = 0;
 	*bus_ib_quota = 0;
@@ -135,6 +136,11 @@
 		pipe = mixer->stage_pipe[i];
 		if (pipe == NULL)
 			continue;
+		if (pipe->is_fg) {
+			ab_total = 0;
+			ib_total = 0;
+			max_clk_rate = 0;
+		}
 
 		quota = fps * pipe->src.w * pipe->src.h;
 		if (pipe->src_fmt->chroma_sample == MDSS_MDP_CHROMA_420)
@@ -161,12 +167,17 @@
 		pr_debug("mixer=%d pnum=%d clk_rate=%u bus ab=%u ib=%u\n",
 			 mixer->num, pipe->num, rate, quota, ib_quota);
 
-		*bus_ab_quota += quota >> MDSS_MDP_BUS_FACTOR_SHIFT;
-		*bus_ib_quota += ib_quota >> MDSS_MDP_BUS_FACTOR_SHIFT;
-		if (rate > *clk_rate)
-			*clk_rate = rate;
+		ab_total += quota >> MDSS_MDP_BUS_FACTOR_SHIFT;
+		ib_total += ib_quota >> MDSS_MDP_BUS_FACTOR_SHIFT;
+		if (rate > max_clk_rate)
+			max_clk_rate = rate;
 	}
 
+	*bus_ab_quota += ab_total;
+	*bus_ib_quota += ib_total;
+	if (max_clk_rate > *clk_rate)
+		*clk_rate = max_clk_rate;
+
 	pr_debug("final mixer=%d clk_rate=%u bus ab=%u ib=%u\n", mixer->num,
 		 *clk_rate, *bus_ab_quota, *bus_ib_quota);
 }
diff --git a/drivers/video/msm/mdss/mdss_mdp_intf_video.c b/drivers/video/msm/mdss/mdss_mdp_intf_video.c
index 4757c63..a6a6d59 100644
--- a/drivers/video/msm/mdss/mdss_mdp_intf_video.c
+++ b/drivers/video/msm/mdss/mdss_mdp_intf_video.c
@@ -16,6 +16,9 @@
 #include "mdss_fb.h"
 #include "mdss_mdp.h"
 
+/* wait for at most 2 vsync for lowest refresh rate (24hz) */
+#define VSYNC_TIMEOUT msecs_to_jiffies(84)
+
 /* intf timing settings */
 struct intf_timing_params {
 	u32 width;
@@ -41,7 +44,6 @@
 	u8 ref_cnt;
 
 	u8 timegen_en;
-	struct completion pp_comp;
 	struct completion vsync_comp;
 
 	atomic_t vsync_ref;
@@ -236,21 +238,6 @@
 	return 0;
 }
 
-static void mdss_mdp_video_pp_intr_done(void *arg)
-{
-	struct mdss_mdp_ctl *ctl = arg;
-	struct mdss_mdp_video_ctx *ctx = ctl->priv_data;
-
-	if (!ctx) {
-		pr_err("invalid ctx\n");
-		return;
-	}
-
-	pr_debug("intr mixer=%d\n", ctx->pp_num);
-
-	complete(&ctx->pp_comp);
-}
-
 static void mdss_mdp_video_vsync_intr_done(void *arg)
 {
 	struct mdss_mdp_ctl *ctl = arg;
@@ -273,27 +260,6 @@
 	spin_unlock(&ctx->vsync_lock);
 }
 
-static int mdss_mdp_video_prepare(struct mdss_mdp_ctl *ctl, void *arg)
-{
-	struct mdss_mdp_video_ctx *ctx;
-
-	ctx = (struct mdss_mdp_video_ctx *) ctl->priv_data;
-	if (!ctx) {
-		pr_err("invalid ctx\n");
-		return -ENODEV;
-	}
-
-	if (ctx->timegen_en) {
-		INIT_COMPLETION(ctx->pp_comp);
-		pr_debug("waiting for ping pong %d done\n", ctx->pp_num);
-		mdss_mdp_irq_enable(MDSS_MDP_IRQ_PING_PONG_COMP, ctx->pp_num);
-		wait_for_completion_interruptible(&ctx->pp_comp);
-		mdss_mdp_irq_disable(MDSS_MDP_IRQ_PING_PONG_COMP, ctx->pp_num);
-	}
-
-	return 0;
-}
-
 static int mdss_mdp_video_display(struct mdss_mdp_ctl *ctl, void *arg)
 {
 	struct mdss_mdp_video_ctx *ctx;
@@ -322,7 +288,9 @@
 		wmb();
 	}
 
-	wait_for_completion(&ctx->vsync_comp);
+	rc = wait_for_completion_interruptible_timeout(&ctx->vsync_comp,
+			VSYNC_TIMEOUT);
+	WARN(rc <= 0, "vsync timed out (%d) ctl=%d\n", rc, ctl->num);
 
 	if (!ctx->timegen_en) {
 		ctx->timegen_en = true;
@@ -366,15 +334,12 @@
 	}
 	ctl->priv_data = ctx;
 	ctx->pp_num = mixer->num;
-	init_completion(&ctx->pp_comp);
 	init_completion(&ctx->vsync_comp);
 	spin_lock_init(&ctx->vsync_lock);
 	atomic_set(&ctx->vsync_ref, 0);
 
 	mdss_mdp_set_intr_callback(MDSS_MDP_IRQ_INTF_VSYNC, ctl->intf_num,
 				   mdss_mdp_video_vsync_intr_done, ctl);
-	mdss_mdp_set_intr_callback(MDSS_MDP_IRQ_PING_PONG_COMP, ctx->pp_num,
-				   mdss_mdp_video_pp_intr_done, ctl);
 
 	itp.width = pinfo->xres + pinfo->lcdc.xres_pad;
 	itp.height = pinfo->yres + pinfo->lcdc.yres_pad;
@@ -397,7 +362,6 @@
 	}
 
 	ctl->stop_fnc = mdss_mdp_video_stop;
-	ctl->prepare_fnc = mdss_mdp_video_prepare;
 	ctl->display_fnc = mdss_mdp_video_display;
 	ctl->set_vsync_handler = mdss_mdp_video_set_vsync_handler;
 
diff --git a/drivers/video/msm/mdss/mdss_mdp_pipe.c b/drivers/video/msm/mdss/mdss_mdp_pipe.c
index 459cf14..ade7cb4 100644
--- a/drivers/video/msm/mdss/mdss_mdp_pipe.c
+++ b/drivers/video/msm/mdss/mdss_mdp_pipe.c
@@ -80,16 +80,21 @@
 static int mdss_mdp_smp_reserve(struct mdss_mdp_pipe *pipe)
 {
 	u32 num_blks = 0, reserved = 0;
-	int i;
+	struct mdss_mdp_plane_sizes ps;
+	int i, rc;
 
-	if ((pipe->src_planes.num_planes > 1) &&
-	    (pipe->type == MDSS_MDP_PIPE_TYPE_RGB))
+	rc = mdss_mdp_get_plane_sizes(pipe->src_fmt->format, pipe->src.w,
+				pipe->src.h, &ps);
+	if (rc)
+		return rc;
+
+	if ((ps.num_planes > 1) && (pipe->type == MDSS_MDP_PIPE_TYPE_RGB))
 		return -EINVAL;
 
 	mutex_lock(&mdss_mdp_smp_lock);
-	for (i = 0; i < pipe->src_planes.num_planes; i++) {
-		num_blks = DIV_ROUND_UP(2 * pipe->src_planes.ystride[i],
-					mdss_res->smp_mb_size);
+	for (i = 0; i < ps.num_planes; i++) {
+		num_blks = DIV_ROUND_UP(2 * ps.ystride[i],
+			mdss_res->smp_mb_size);
 
 		pr_debug("reserving %d mmb for pnum=%d plane=%d\n",
 				num_blks, pipe->num, i);
diff --git a/drivers/video/msm/mipi_NT35510.c b/drivers/video/msm/mipi_NT35510.c
index d75198a..6d0bf7c 100644
--- a/drivers/video/msm/mipi_NT35510.c
+++ b/drivers/video/msm/mipi_NT35510.c
@@ -473,6 +473,8 @@
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
 	static int rotate;
+	struct dcs_cmd_req cmdreq;
+
 	mfd = platform_get_drvdata(pdev);
 	if (!mfd)
 		return -ENODEV;
@@ -490,25 +492,41 @@
 	if (mipi_nt35510_pdata && mipi_nt35510_pdata->rotate_panel)
 		rotate = mipi_nt35510_pdata->rotate_panel();
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	if (mipi->mode == DSI_VIDEO_MODE) {
-		mipi_dsi_cmds_tx(&nt35510_tx_buf,
-			nt35510_video_display_on_cmds,
-			ARRAY_SIZE(nt35510_video_display_on_cmds));
+		cmdreq.cmds = nt35510_video_display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(nt35510_video_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 
 		if (rotate) {
-			mipi_dsi_cmds_tx(&nt35510_tx_buf,
-				nt35510_video_display_on_cmds_rotate,
-			ARRAY_SIZE(nt35510_video_display_on_cmds_rotate));
+			cmdreq.cmds = nt35510_video_display_on_cmds_rotate;
+			cmdreq.cmds_cnt =
+			ARRAY_SIZE(nt35510_video_display_on_cmds_rotate);
+			cmdreq.flags = CMD_REQ_COMMIT;
+			cmdreq.rlen = 0;
+			cmdreq.cb = NULL;
+			mipi_dsi_cmdlist_put(&cmdreq);
 		}
 	} else if (mipi->mode == DSI_CMD_MODE) {
-		mipi_dsi_cmds_tx(&nt35510_tx_buf,
-			nt35510_cmd_display_on_cmds,
-			ARRAY_SIZE(nt35510_cmd_display_on_cmds));
+		cmdreq.cmds = nt35510_cmd_display_on_cmds;
+		cmdreq.cmds_cnt =
+			ARRAY_SIZE(nt35510_cmd_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 
 		if (rotate) {
-			mipi_dsi_cmds_tx(&nt35510_tx_buf,
-				nt35510_cmd_display_on_cmds_rotate,
-			ARRAY_SIZE(nt35510_cmd_display_on_cmds_rotate));
+			cmdreq.cmds = nt35510_cmd_display_on_cmds_rotate;
+			cmdreq.cmds_cnt =
+				ARRAY_SIZE(nt35510_cmd_display_on_cmds_rotate);
+			cmdreq.flags = CMD_REQ_COMMIT;
+			cmdreq.rlen = 0;
+			cmdreq.cb = NULL;
+			mipi_dsi_cmdlist_put(&cmdreq);
 		}
 	}
 
@@ -518,6 +536,7 @@
 static int mipi_nt35510_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	pr_debug("mipi_nt35510_lcd_off E\n");
 
@@ -528,8 +547,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&nt35510_tx_buf, nt35510_display_off_cmds,
-			ARRAY_SIZE(nt35510_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = nt35510_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(nt35510_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	pr_debug("mipi_nt35510_lcd_off X\n");
 	return 0;
diff --git a/drivers/video/msm/mipi_dsi.c b/drivers/video/msm/mipi_dsi.c
index cc19555..35b6ae6 100644
--- a/drivers/video/msm/mipi_dsi.c
+++ b/drivers/video/msm/mipi_dsi.c
@@ -106,10 +106,6 @@
 
 	ret = panel_next_off(pdev);
 
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(0);
-#endif
-
 	mipi_dsi_clk_disable();
 
 	/* disbale dsi engine */
@@ -313,10 +309,6 @@
 		mipi_dsi_unprepare_clocks();
 	}
 
-#ifdef CONFIG_MSM_BUS_SCALING
-	mdp_bus_scale_update_request(2);
-#endif
-
 	if (mdp_rev >= MDP_REV_41)
 		mutex_unlock(&mfd->dma->ov_mutex);
 	else
diff --git a/drivers/video/msm/mipi_dsi.h b/drivers/video/msm/mipi_dsi.h
index d4d7288..02b42bc 100644
--- a/drivers/video/msm/mipi_dsi.h
+++ b/drivers/video/msm/mipi_dsi.h
@@ -267,11 +267,13 @@
 #define CMD_REQ_COMMIT	0x0002
 #define CMD_CLK_CTRL	0x0004
 #define CMD_REQ_NO_MAX_PKT_SIZE 0x0008
+#define CMD_MDP3_CMD_PANEL 0x80000000  /* mdp3 only */
 
 struct dcs_cmd_req {
 	struct dsi_cmd_desc *cmds;
 	int cmds_cnt;
 	u32 flags;
+	struct dsi_buf *rbuf;
 	int rlen;	/* rx length */
 	fxn cb;
 };
@@ -291,14 +293,7 @@
 void mipi_dsi_bist_ctrl(void);
 int mipi_dsi_buf_alloc(struct dsi_buf *, int size);
 int mipi_dsi_cmd_dma_add(struct dsi_buf *dp, struct dsi_cmd_desc *cm);
-int mipi_dsi_cmds_tx(struct dsi_buf *dp, struct dsi_cmd_desc *cmds, int cnt);
-
-int mipi_dsi_cmd_dma_tx(struct dsi_buf *dp);
 int mipi_dsi_cmd_reg_tx(uint32 data);
-int mipi_dsi_cmds_rx(struct msm_fb_data_type *mfd,
-			struct dsi_buf *tp, struct dsi_buf *rp,
-			struct dsi_cmd_desc *cmds, int len);
-int mipi_dsi_cmd_dma_rx(struct dsi_buf *tp, int rlen);
 void mipi_dsi_host_init(struct mipi_panel_info *pinfo);
 void mipi_dsi_op_mode_config(int mode);
 void mipi_dsi_cmd_mode_ctrl(int enable);
diff --git a/drivers/video/msm/mipi_dsi_host.c b/drivers/video/msm/mipi_dsi_host.c
index 96bfb9f..559c7a1 100644
--- a/drivers/video/msm/mipi_dsi_host.c
+++ b/drivers/video/msm/mipi_dsi_host.c
@@ -1130,11 +1130,15 @@
 	return 4;
 }
 
+static int mipi_dsi_cmd_dma_tx(struct dsi_buf *tp);
+static int mipi_dsi_cmd_dma_rx(struct dsi_buf *rp, int rlen);
+
 /*
  * mipi_dsi_cmds_tx:
  * thread context only
  */
-int mipi_dsi_cmds_tx(struct dsi_buf *tp, struct dsi_cmd_desc *cmds, int cnt)
+static int mipi_dsi_cmds_tx(struct dsi_buf *tp,
+			struct dsi_cmd_desc *cmds, int cnt)
 {
 	struct dsi_cmd_desc *cm;
 	uint32 dsi_ctrl, ctrl;
@@ -1187,123 +1191,8 @@
  * len should be either 4 or 8
  * any return data more than MIPI_DSI_LEN need to be break down
  * to multiple transactions.
- *
- * ov_mutex need to be acquired before call this function.
  */
-int mipi_dsi_cmds_rx(struct msm_fb_data_type *mfd,
-			struct dsi_buf *tp, struct dsi_buf *rp,
-			struct dsi_cmd_desc *cmds, int rlen)
-{
-	int cnt, len, diff, pkt_size;
-	char cmd;
-
-	if (mfd->panel_info.mipi.no_max_pkt_size) {
-		/* Only support rlen = 4*n */
-		rlen += 3;
-		rlen &= ~0x03;
-	}
-
-	len = rlen;
-	diff = 0;
-
-	if (len <= 2)
-		cnt = 4;	/* short read */
-	else {
-		if (len > MIPI_DSI_LEN)
-			len = MIPI_DSI_LEN;	/* 8 bytes at most */
-
-		len = (len + 3) & ~0x03; /* len 4 bytes align */
-		diff = len - rlen;
-		/*
-		 * add extra 2 bytes to len to have overall
-		 * packet size is multipe by 4. This also make
-		 * sure 4 bytes dcs headerlocates within a
-		 * 32 bits register after shift in.
-		 * after all, len should be either 6 or 10.
-		 */
-		len += 2;
-		cnt = len + 6; /* 4 bytes header + 2 bytes crc */
-	}
-
-	if (mfd->panel_info.type == MIPI_CMD_PANEL) {
-		/* make sure mdp dma is not txing pixel data */
-#ifdef CONFIG_FB_MSM_MDP303
-			mdp3_dsi_cmd_dma_busy_wait(mfd);
-#endif
-	}
-
-	if (!mfd->panel_info.mipi.no_max_pkt_size) {
-		/* packet size need to be set at every read */
-		pkt_size = len;
-		max_pktsize[0] = pkt_size;
-		mipi_dsi_enable_irq(DSI_CMD_TERM);
-		mipi_dsi_buf_init(tp);
-		mipi_dsi_cmd_dma_add(tp, pkt_size_cmd);
-		mipi_dsi_cmd_dma_tx(tp);
-	}
-
-	mipi_dsi_enable_irq(DSI_CMD_TERM);
-	mipi_dsi_buf_init(tp);
-	mipi_dsi_cmd_dma_add(tp, cmds);
-
-	/* transmit read comamnd to client */
-	mipi_dsi_cmd_dma_tx(tp);
-
-	mipi_dsi_disable_irq(DSI_CMD_TERM);
-	/*
-	 * once cmd_dma_done interrupt received,
-	 * return data from client is ready and stored
-	 * at RDBK_DATA register already
-	 */
-	mipi_dsi_buf_init(rp);
-	if (mfd->panel_info.mipi.no_max_pkt_size) {
-		/*
-		 * expect rlen = n * 4
-		 * short alignement for start addr
-		 */
-		rp->data += 2;
-	}
-
-	mipi_dsi_cmd_dma_rx(rp, cnt);
-
-	if (mfd->panel_info.mipi.no_max_pkt_size) {
-		/*
-		 * remove extra 2 bytes from previous
-		 * rx transaction at shift register
-		 * which was inserted during copy
-		 * shift registers to rx buffer
-		 * rx payload start from long alignment addr
-		 */
-		rp->data += 2;
-	}
-
-	cmd = rp->data[0];
-	switch (cmd) {
-	case DTYPE_ACK_ERR_RESP:
-		pr_debug("%s: rx ACK_ERR_PACLAGE\n", __func__);
-		break;
-	case DTYPE_GEN_READ1_RESP:
-	case DTYPE_DCS_READ1_RESP:
-		mipi_dsi_short_read1_resp(rp);
-		break;
-	case DTYPE_GEN_READ2_RESP:
-	case DTYPE_DCS_READ2_RESP:
-		mipi_dsi_short_read2_resp(rp);
-		break;
-	case DTYPE_GEN_LREAD_RESP:
-	case DTYPE_DCS_LREAD_RESP:
-		mipi_dsi_long_read_resp(rp);
-		rp->len -= 2; /* extra 2 bytes added */
-		rp->len -= diff; /* align bytes */
-		break;
-	default:
-		break;
-	}
-
-	return rp->len;
-}
-
-int mipi_dsi_cmds_rx_new(struct dsi_buf *tp, struct dsi_buf *rp,
+static int mipi_dsi_cmds_rx(struct dsi_buf *tp, struct dsi_buf *rp,
 			struct dcs_cmd_req *req, int rlen)
 {
 	struct dsi_cmd_desc *cmds;
@@ -1359,7 +1248,6 @@
 	/* transmit read comamnd to client */
 	mipi_dsi_cmd_dma_tx(tp);
 
-	mipi_dsi_disable_irq(DSI_CMD_TERM);
 	/*
 	 * once cmd_dma_done interrupt received,
 	 * return data from client is ready and stored
@@ -1413,7 +1301,7 @@
 	return rp->len;
 }
 
-int mipi_dsi_cmd_dma_tx(struct dsi_buf *tp)
+static int mipi_dsi_cmd_dma_tx(struct dsi_buf *tp)
 {
 
 	unsigned long flags;
@@ -1460,7 +1348,7 @@
 	return tp->len;
 }
 
-int mipi_dsi_cmd_dma_rx(struct dsi_buf *rp, int rlen)
+static int mipi_dsi_cmd_dma_rx(struct dsi_buf *rp, int rlen)
 {
 	uint32 *lp, data;
 	int i, off, cnt;
@@ -1565,12 +1453,17 @@
 	struct dsi_buf *rp;
 
 	mipi_dsi_buf_init(&dsi_tx_buf);
-	mipi_dsi_buf_init(&dsi_rx_buf);
 
 	tp = &dsi_tx_buf;
-	rp = &dsi_rx_buf;
 
-	len = mipi_dsi_cmds_rx_new(tp, rp, req, req->rlen);
+	if (req->rbuf)
+		rp = req->rbuf;
+	else
+		rp = &dsi_rx_buf;
+
+	mipi_dsi_buf_init(rp);
+
+	len = mipi_dsi_cmds_rx(tp, rp, req, req->rlen);
 	dp = (u32 *)rp->data;
 
 	if (req->cb)
diff --git a/drivers/video/msm/mipi_novatek.c b/drivers/video/msm/mipi_novatek.c
index 68bc65e..7e0a46e 100644
--- a/drivers/video/msm/mipi_novatek.c
+++ b/drivers/video/msm/mipi_novatek.c
@@ -314,10 +314,12 @@
 {
 	struct dcs_cmd_req cmdreq;
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	cmdreq.cmds = &novatek_manufacture_id_cmd;
 	cmdreq.cmds_cnt = 1;
 	cmdreq.flags = CMD_REQ_RX | CMD_REQ_COMMIT;
 	cmdreq.rlen = 3;
+	cmdreq.rbuf = NULL;
 	cmdreq.cb = mipi_novatek_manufacture_cb; /* call back */
 	mipi_dsi_cmdlist_put(&cmdreq);
 	/*
diff --git a/drivers/video/msm/mipi_orise.c b/drivers/video/msm/mipi_orise.c
index d1d6956..ea37def 100644
--- a/drivers/video/msm/mipi_orise.c
+++ b/drivers/video/msm/mipi_orise.c
@@ -53,6 +53,7 @@
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
 	struct msm_panel_info *pinfo;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 	if (!mfd)
@@ -63,12 +64,21 @@
 	pinfo = &mfd->panel_info;
 	mipi  = &mfd->panel_info.mipi;
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	if (mipi->mode == DSI_VIDEO_MODE) {
-		mipi_dsi_cmds_tx(&orise_tx_buf, orise_video_on_cmds,
-			ARRAY_SIZE(orise_video_on_cmds));
+		cmdreq.cmds = orise_video_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(orise_video_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	} else {
-		mipi_dsi_cmds_tx(&orise_tx_buf, orise_cmd_on_cmds,
-			ARRAY_SIZE(orise_cmd_on_cmds));
+		cmdreq.cmds = orise_cmd_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(orise_cmd_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 
 		mipi_dsi_cmd_bta_sw_trigger(); /* clean up ack_err_status */
 	}
@@ -79,6 +89,7 @@
 static int mipi_orise_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -87,8 +98,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&orise_tx_buf, orise_display_off_cmds,
-			ARRAY_SIZE(orise_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = orise_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(orise_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
diff --git a/drivers/video/msm/mipi_renesas.c b/drivers/video/msm/mipi_renesas.c
index c842672..0d6b4be 100644
--- a/drivers/video/msm/mipi_renesas.c
+++ b/drivers/video/msm/mipi_renesas.c
@@ -1122,6 +1122,7 @@
 {
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 	mipi  = &mfd->panel_info.mipi;
@@ -1131,24 +1132,47 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_sleep_off_cmds,
-			ARRAY_SIZE(renesas_sleep_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = renesas_sleep_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(renesas_sleep_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	mipi_set_tx_power_mode(1);
-	mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_display_on_cmds,
-			ARRAY_SIZE(renesas_display_on_cmds));
+
+	cmdreq.cmds = renesas_display_on_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(renesas_display_on_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	if (cpu_is_msm7x25a() || cpu_is_msm7x25aa() || cpu_is_msm7x25ab()) {
-		mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_hvga_on_cmds,
-			ARRAY_SIZE(renesas_hvga_on_cmds));
+		cmdreq.cmds = renesas_hvga_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(renesas_hvga_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	}
 
-	if (mipi->mode == DSI_VIDEO_MODE)
-		mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_video_on_cmds,
-			ARRAY_SIZE(renesas_video_on_cmds));
-	else
-		mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_cmd_on_cmds,
-			ARRAY_SIZE(renesas_cmd_on_cmds));
+	if (mipi->mode == DSI_VIDEO_MODE) {
+		cmdreq.cmds = renesas_video_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(renesas_video_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
+	} else {
+		cmdreq.cmds = renesas_cmd_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(renesas_cmd_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
+	}
 	mipi_set_tx_power_mode(0);
 
 	return 0;
@@ -1157,6 +1181,7 @@
 static int mipi_renesas_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -1165,8 +1190,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&renesas_tx_buf, renesas_display_off_cmds,
-			ARRAY_SIZE(renesas_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = renesas_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(renesas_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
diff --git a/drivers/video/msm/mipi_simulator.c b/drivers/video/msm/mipi_simulator.c
index c751472..cac927b 100644
--- a/drivers/video/msm/mipi_simulator.c
+++ b/drivers/video/msm/mipi_simulator.c
@@ -36,6 +36,7 @@
 {
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 	mipi  = &mfd->panel_info.mipi;
@@ -48,9 +49,14 @@
 	pr_debug("%s:%d, debug info (mode) : %d", __func__, __LINE__,
 		 mipi->mode);
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	if (mipi->mode == DSI_VIDEO_MODE) {
-		mipi_dsi_cmds_tx(&simulator_tx_buf, display_on_cmds,
-			ARRAY_SIZE(display_on_cmds));
+		cmdreq.cmds = display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	} else {
 		pr_err("%s:%d, CMD MODE NOT SUPPORTED", __func__, __LINE__);
 		return -EINVAL;
@@ -63,6 +69,7 @@
 {
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 	mipi  = &mfd->panel_info.mipi;
@@ -74,9 +81,14 @@
 
 	pr_debug("%s:%d, debug info", __func__, __LINE__);
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	if (mipi->mode == DSI_VIDEO_MODE) {
-		mipi_dsi_cmds_tx(&simulator_tx_buf, display_off_cmds,
-			ARRAY_SIZE(display_off_cmds));
+		cmdreq.cmds = display_off_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(display_off_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	} else {
 		pr_debug("%s:%d, DONT REACH HERE", __func__, __LINE__);
 		return -EINVAL;
diff --git a/drivers/video/msm/mipi_tc358764_dsi2lvds.c b/drivers/video/msm/mipi_tc358764_dsi2lvds.c
index 1583168..5db4fd2 100644
--- a/drivers/video/msm/mipi_tc358764_dsi2lvds.c
+++ b/drivers/video/msm/mipi_tc358764_dsi2lvds.c
@@ -243,20 +243,25 @@
 {
 	u32 data;
 	int len = 4;
+	struct dcs_cmd_req cmdreq;
 	struct dsi_cmd_desc cmd_read_reg = {
 		DTYPE_GEN_READ2, 1, 0, 1, 0, /* cmd 0x24 */
 			sizeof(reg), (char *) &reg};
 
-	mipi_dsi_buf_init(&d2l_tx_buf);
 	mipi_dsi_buf_init(&d2l_rx_buf);
 
-	/* mutex had been acquired at mipi_dsi_on */
-	len = mipi_dsi_cmds_rx(mfd, &d2l_tx_buf, &d2l_rx_buf,
-			       &cmd_read_reg, len);
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = &cmd_read_reg;
+	cmdreq.cmds_cnt = 1;
+	cmdreq.flags = CMD_REQ_RX | CMD_REQ_COMMIT | CMD_REQ_NO_MAX_PKT_SIZE;
+	cmdreq.rbuf = &d2l_rx_buf;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	data = *(u32 *)d2l_rx_buf.data;
 
-	if (len != 4)
+	if (d2l_rx_buf.len != 4)
 		pr_err("%s: invalid rlen=%d, expecting 4.\n", __func__, len);
 
 	pr_debug("%s: reg=0x%x.data=0x%08x.\n", __func__, reg, data);
@@ -274,6 +279,7 @@
 static int mipi_d2l_write_reg(struct msm_fb_data_type *mfd, u16 reg, u32 data)
 {
 	struct wr_cmd_payload payload;
+	struct dcs_cmd_req cmdreq;
 	struct dsi_cmd_desc cmd_write_reg = {
 		DTYPE_GEN_LWRITE, 1, 0, 0, 0,
 			sizeof(payload), (char *)&payload};
@@ -281,8 +287,13 @@
 	payload.addr = reg;
 	payload.data = data;
 
-	/* mutex had been acquried at dsi_on */
-	mipi_dsi_cmds_tx(&d2l_tx_buf, &cmd_write_reg, 1);
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = &cmd_write_reg;
+	cmdreq.cmds_cnt = 1;
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	pr_debug("%s: reg=0x%x. data=0x%x.\n", __func__, reg, data);
 
diff --git a/drivers/video/msm/mipi_toshiba.c b/drivers/video/msm/mipi_toshiba.c
index 520c67b..63504c9 100644
--- a/drivers/video/msm/mipi_toshiba.c
+++ b/drivers/video/msm/mipi_toshiba.c
@@ -184,6 +184,7 @@
 static int mipi_toshiba_lcd_on(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -192,16 +193,23 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	if (TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WVGA_PT)
-		mipi_dsi_cmds_tx(&toshiba_tx_buf,
-			toshiba_wvga_display_on_cmds,
-			ARRAY_SIZE(toshiba_wvga_display_on_cmds));
-	else if (TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WSVGA_PT ||
-		TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WUXGA)
-		mipi_dsi_cmds_tx(&toshiba_tx_buf,
-			toshiba_wsvga_display_on_cmds,
-			ARRAY_SIZE(toshiba_wsvga_display_on_cmds));
-	else
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	if (TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WVGA_PT) {
+		cmdreq.cmds = toshiba_wvga_display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(toshiba_wvga_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
+	} else if (TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WSVGA_PT ||
+		TM_GET_PID(mfd->panel.id) == MIPI_DSI_PANEL_WUXGA) {
+		cmdreq.cmds = toshiba_wsvga_display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(toshiba_wsvga_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
+	} else
 		return -EINVAL;
 
 	return 0;
@@ -210,6 +218,7 @@
 static int mipi_toshiba_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -218,8 +227,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&toshiba_tx_buf, toshiba_display_off_cmds,
-			ARRAY_SIZE(toshiba_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = toshiba_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(toshiba_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
diff --git a/drivers/video/msm/mipi_truly.c b/drivers/video/msm/mipi_truly.c
index fd2a3ea..ea9c047 100644
--- a/drivers/video/msm/mipi_truly.c
+++ b/drivers/video/msm/mipi_truly.c
@@ -107,6 +107,7 @@
 static int mipi_truly_lcd_on(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -116,8 +117,14 @@
 		return -EINVAL;
 
 	msleep(20);
-	mipi_dsi_cmds_tx(&truly_tx_buf, truly_display_on_cmds,
-			ARRAY_SIZE(truly_display_on_cmds));
+
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = truly_display_on_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(truly_display_on_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
@@ -125,6 +132,7 @@
 static int mipi_truly_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -133,8 +141,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&truly_tx_buf, truly_display_off_cmds,
-			ARRAY_SIZE(truly_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = truly_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(truly_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
diff --git a/drivers/video/msm/mipi_truly_tft540960_1_e.c b/drivers/video/msm/mipi_truly_tft540960_1_e.c
index 50db66e..7f001eb 100644
--- a/drivers/video/msm/mipi_truly_tft540960_1_e.c
+++ b/drivers/video/msm/mipi_truly_tft540960_1_e.c
@@ -679,6 +679,7 @@
 {
 	struct msm_fb_data_type *mfd;
 	struct mipi_panel_info *mipi;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -692,14 +693,21 @@
 	pr_info("%s: mode = %d\n", __func__, mipi->mode);
 	msleep(120);
 
+	memset(&cmdreq, 0, sizeof(cmdreq));
 	if (mipi->mode == DSI_VIDEO_MODE) {
-		mipi_dsi_cmds_tx(&truly_tx_buf,
-			truly_video_display_on_cmds,
-			ARRAY_SIZE(truly_video_display_on_cmds));
+		cmdreq.cmds = truly_video_display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(truly_video_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	} else if (mipi->mode == DSI_CMD_MODE) {
-		mipi_dsi_cmds_tx(&truly_tx_buf,
-			truly_cmd_display_on_cmds,
-			ARRAY_SIZE(truly_cmd_display_on_cmds));
+		cmdreq.cmds = truly_cmd_display_on_cmds;
+		cmdreq.cmds_cnt = ARRAY_SIZE(truly_cmd_display_on_cmds);
+		cmdreq.flags = CMD_REQ_COMMIT;
+		cmdreq.rlen = 0;
+		cmdreq.cb = NULL;
+		mipi_dsi_cmdlist_put(&cmdreq);
 	}
 
 	return 0;
@@ -708,6 +716,7 @@
 static int mipi_truly_lcd_off(struct platform_device *pdev)
 {
 	struct msm_fb_data_type *mfd;
+	struct dcs_cmd_req cmdreq;
 
 	mfd = platform_get_drvdata(pdev);
 
@@ -716,8 +725,13 @@
 	if (mfd->key != MFD_KEY)
 		return -EINVAL;
 
-	mipi_dsi_cmds_tx(&truly_tx_buf, truly_display_off_cmds,
-			ARRAY_SIZE(truly_display_off_cmds));
+	memset(&cmdreq, 0, sizeof(cmdreq));
+	cmdreq.cmds = truly_display_off_cmds;
+	cmdreq.cmds_cnt = ARRAY_SIZE(truly_display_off_cmds);
+	cmdreq.flags = CMD_REQ_COMMIT;
+	cmdreq.rlen = 0;
+	cmdreq.cb = NULL;
+	mipi_dsi_cmdlist_put(&cmdreq);
 
 	return 0;
 }
diff --git a/drivers/video/msm/msm_fb.c b/drivers/video/msm/msm_fb.c
index 993ec01..b7e0bbf 100644
--- a/drivers/video/msm/msm_fb.c
+++ b/drivers/video/msm/msm_fb.c
@@ -1286,27 +1286,8 @@
 	var->yres_virtual = panel_info->yres * mfd->fb_page +
 		((PAGE_SIZE - remainder)/fix->line_length) * mfd->fb_page;
 	var->bits_per_pixel = bpp * 8;	/* FrameBuffer color depth */
-	if (mfd->dest == DISPLAY_LCD) {
-		if (panel_info->type == MDDI_PANEL && panel_info->mddi.is_type1)
-			var->reserved[4] = panel_info->lcd.refx100 / (100 * 2);
-		else
-			var->reserved[4] = panel_info->lcd.refx100 / 100;
-	} else {
-		if (panel_info->type == MIPI_VIDEO_PANEL) {
-			var->reserved[4] = panel_info->mipi.frame_rate;
-		} else {
-			var->reserved[4] = panel_info->clk_rate /
-				((panel_info->lcdc.h_back_porch +
-				  panel_info->lcdc.h_front_porch +
-				  panel_info->lcdc.h_pulse_width +
-				  panel_info->xres) *
-				 (panel_info->lcdc.v_back_porch +
-				  panel_info->lcdc.v_front_porch +
-				  panel_info->lcdc.v_pulse_width +
-				  panel_info->yres));
-		}
-	}
-	pr_debug("reserved[4] %u\n", var->reserved[4]);
+
+	var->reserved[4] = mdp_get_panel_framerate(mfd);
 
 		/*
 		 * id field for fb app
diff --git a/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_interrupt_handler.c b/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_interrupt_handler.c
index 78f96c8..d189408 100644
--- a/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_interrupt_handler.c
+++ b/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_interrupt_handler.c
@@ -1259,7 +1259,7 @@
 				output_vcd_frm->flags |=
 					VCD_FRAME_FLAG_DATACORRUPT;
 		}
-		if (decoder->codec.codec != VCD_CODEC_H264 ||
+		if (decoder->codec.codec != VCD_CODEC_H264 &&
 			decoder->codec.codec != VCD_CODEC_MPEG2)
 			output_vcd_frm->flags &= ~VCD_FRAME_FLAG_DATACORRUPT;
 		output_vcd_frm->ip_frm_tag = dec_disp_info->tag_top;
diff --git a/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_utils.h b/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_utils.h
index 6817101..4963874 100644
--- a/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_utils.h
+++ b/drivers/video/msm/vidc/1080p/ddl/vcd_ddl_utils.h
@@ -38,13 +38,15 @@
 #define DDL_MSG_LOW(x...)    printk(KERN_INFO x)
 #define DDL_MSG_MED(x...)    printk(KERN_INFO x)
 #define DDL_MSG_HIGH(x...)   printk(KERN_INFO x)
+#define DDL_MSG_ERROR(x...)  printk(KERN_INFO x)
 #else
 #define DDL_MSG_LOW(x...)
 #define DDL_MSG_MED(x...)
 #define DDL_MSG_HIGH(x...)
+#define DDL_MSG_ERROR(x...)
 #endif
 
-#define DDL_MSG_ERROR(x...)  printk(KERN_INFO x)
+
 #define DDL_MSG_FATAL(x...)  printk(KERN_INFO x)
 
 #define DDL_ALIGN_SIZE(sz, guard_bytes, align_mask) \
diff --git a/drivers/video/msm/vidc/common/vcd/vcd_util.h b/drivers/video/msm/vidc/common/vcd/vcd_util.h
index 7164029..ed7fc1a 100644
--- a/drivers/video/msm/vidc/common/vcd/vcd_util.h
+++ b/drivers/video/msm/vidc/common/vcd/vcd_util.h
@@ -20,16 +20,16 @@
 #define VCD_MSG_LOW(xx_fmt...)		printk(KERN_INFO "\n\t* " xx_fmt)
 #define VCD_MSG_MED(xx_fmt...)		printk(KERN_INFO "\n  * " xx_fmt)
 #define VCD_MSG_HIGH(xx_fmt...)		printk(KERN_WARNING "\n" xx_fmt)
-
+#define VCD_MSG_ERROR(xx_fmt...)	printk(KERN_ERR "\n err: " xx_fmt)
 #else
 
 #define VCD_MSG_LOW(xx_fmt...)
 #define VCD_MSG_MED(xx_fmt...)
 #define VCD_MSG_HIGH(xx_fmt...)
-
+#define VCD_MSG_ERROR(xx_fmt...)
 #endif
 
-#define VCD_MSG_ERROR(xx_fmt...)	printk(KERN_ERR "\n err: " xx_fmt)
+
 #define VCD_MSG_FATAL(xx_fmt...)	printk(KERN_ERR "\n<FATAL> " xx_fmt)
 
 #define VCD_FAILED_RETURN(rc, xx_fmt...)		\
diff --git a/fs/fuse/file.c b/fs/fuse/file.c
index 504e61b..6e0f58b 100644
--- a/fs/fuse/file.c
+++ b/fs/fuse/file.c
@@ -663,6 +663,42 @@
 			return PTR_ERR(req);
 		}
 	}
+
+#ifdef CONFIG_DMA_CMA
+	if (is_cma_pageblock(page)) {
+		struct page *oldpage = page, *newpage;
+		int err;
+
+		/* make sure that old page is not free in-between the calls */
+		page_cache_get(oldpage);
+
+		newpage = alloc_page(GFP_HIGHUSER);
+		if (!newpage) {
+			page_cache_release(oldpage);
+			return -ENOMEM;
+		}
+
+		err = replace_page_cache_page(oldpage, newpage, GFP_KERNEL);
+		if (err) {
+			__free_page(newpage);
+			page_cache_release(oldpage);
+			return err;
+		}
+
+		/*
+		 * Decrement the count on new page to make page cache the only
+		 * owner of it
+		 */
+		lock_page(newpage);
+		put_page(newpage);
+
+		/* finally release the old page and swap pointers */
+		unlock_page(oldpage);
+		page_cache_release(oldpage);
+		page = newpage;
+	}
+#endif
+
 	page_cache_get(page);
 	req->pages[req->num_pages] = page;
 	req->num_pages++;
diff --git a/include/linux/Kbuild b/include/linux/Kbuild
index 29546b7..46ae59f 100644
--- a/include/linux/Kbuild
+++ b/include/linux/Kbuild
@@ -447,3 +447,4 @@
 header-y += coresight-stm.h
 header-y += ci-bridge-spi.h
 header-y += msm_audio_amrwbplus.h
+header-y += avtimer.h
diff --git a/include/linux/avtimer.h b/include/linux/avtimer.h
new file mode 100644
index 0000000..e68da6a
--- /dev/null
+++ b/include/linux/avtimer.h
@@ -0,0 +1,21 @@
+#ifndef AVTIMER_H
+#define AVTIMER_H
+
+#include <linux/ioctl.h>
+
+#define MAJOR_NUM 100
+
+#define IOCTL_GET_AVTIMER_TICK _IOR(MAJOR_NUM, 0, char *)
+/*
+ * This IOCTL is used read the avtimer tick value.
+ * Avtimer is a 64 bit timer tick, hence the expected
+ * argument is of type uint64_t
+ */
+struct dev_avtimer_data {
+	uint32_t avtimer_msw_phy_addr;
+	uint32_t avtimer_lsw_phy_addr;
+};
+int avcs_core_open(void);
+int avcs_core_disable_power_collapse(int disable);/* true or flase */
+
+#endif
diff --git a/include/linux/blkdev.h b/include/linux/blkdev.h
index e76b0ae..6502841 100644
--- a/include/linux/blkdev.h
+++ b/include/linux/blkdev.h
@@ -282,6 +282,7 @@
 	struct request_list	rq;
 
 	request_fn_proc		*request_fn;
+	request_fn_proc		*urgent_request_fn;
 	make_request_fn		*make_request_fn;
 	prep_rq_fn		*prep_rq_fn;
 	unprep_rq_fn		*unprep_rq_fn;
@@ -365,6 +366,8 @@
 	struct list_head	icq_list;
 
 	struct queue_limits	limits;
+	bool			notified_urgent;
+	bool			dispatched_urgent;
 
 	/*
 	 * sg stuff
@@ -673,6 +676,8 @@
 extern struct request *blk_make_request(struct request_queue *, struct bio *,
 					gfp_t);
 extern void blk_requeue_request(struct request_queue *, struct request *);
+extern int blk_reinsert_request(struct request_queue *q, struct request *rq);
+extern bool blk_reinsert_req_sup(struct request_queue *q);
 extern void blk_add_request_payload(struct request *rq, struct page *page,
 		unsigned int len);
 extern int blk_rq_check_limits(struct request_queue *q, struct request *rq);
@@ -822,6 +827,7 @@
 extern struct request_queue *blk_init_queue(request_fn_proc *, spinlock_t *);
 extern struct request_queue *blk_init_allocated_queue(struct request_queue *,
 						      request_fn_proc *, spinlock_t *);
+extern void blk_urgent_request(struct request_queue *q, request_fn_proc *fn);
 extern void blk_cleanup_queue(struct request_queue *);
 extern void blk_queue_make_request(struct request_queue *, make_request_fn *);
 extern void blk_queue_bounce_limit(struct request_queue *, u64);
diff --git a/include/linux/device.h b/include/linux/device.h
index 9fca83b..40374ce 100644
--- a/include/linux/device.h
+++ b/include/linux/device.h
@@ -35,6 +35,7 @@
 struct bus_type;
 struct device_node;
 struct iommu_ops;
+struct iommu_group;
 
 struct bus_attribute {
 	struct attribute	attr;
@@ -686,6 +687,7 @@
 	const struct attribute_group **groups;	/* optional groups */
 
 	void	(*release)(struct device *dev);
+	struct iommu_group	*iommu_group;
 };
 
 /* Get the wakeup routines, which depend on struct device */
diff --git a/include/linux/elevator.h b/include/linux/elevator.h
index 7d4e035..b36b28f 100644
--- a/include/linux/elevator.h
+++ b/include/linux/elevator.h
@@ -22,6 +22,9 @@
 typedef int (elevator_dispatch_fn) (struct request_queue *, int);
 
 typedef void (elevator_add_req_fn) (struct request_queue *, struct request *);
+typedef int (elevator_reinsert_req_fn) (struct request_queue *,
+					struct request *);
+typedef bool (elevator_is_urgent_fn) (struct request_queue *);
 typedef struct request *(elevator_request_list_fn) (struct request_queue *, struct request *);
 typedef void (elevator_completed_req_fn) (struct request_queue *, struct request *);
 typedef int (elevator_may_queue_fn) (struct request_queue *, int);
@@ -46,6 +49,9 @@
 
 	elevator_dispatch_fn *elevator_dispatch_fn;
 	elevator_add_req_fn *elevator_add_req_fn;
+	elevator_reinsert_req_fn *elevator_reinsert_req_fn;
+	elevator_is_urgent_fn *elevator_is_urgent_fn;
+
 	elevator_activate_req_fn *elevator_activate_req_fn;
 	elevator_deactivate_req_fn *elevator_deactivate_req_fn;
 
@@ -122,6 +128,7 @@
 extern void elv_bio_merged(struct request_queue *q, struct request *,
 				struct bio *);
 extern void elv_requeue_request(struct request_queue *, struct request *);
+extern int elv_reinsert_request(struct request_queue *, struct request *);
 extern struct request *elv_former_request(struct request_queue *, struct request *);
 extern struct request *elv_latter_request(struct request_queue *, struct request *);
 extern int elv_register_queue(struct request_queue *q);
diff --git a/include/linux/epm_adc.h b/include/linux/epm_adc.h
index ca1a425..14cb148 100644
--- a/include/linux/epm_adc.h
+++ b/include/linux/epm_adc.h
@@ -13,55 +13,54 @@
 	int32_t physical;
 };
 
-#ifdef __KERNEL__
 struct epm_psoc_init_resp {
-	u8	cmd;
-	u8	version;
-	u8	compatible_ver;
-	u8	firm_ver[3];
-	u8	num_dev;
-	u8	num_channel;
+	uint8_t	cmd;
+	uint8_t	version;
+	uint8_t	compatible_ver;
+	uint8_t	firm_ver[3];
+	uint8_t	num_dev;
+	uint8_t	num_channel;
 };
 
 struct epm_psoc_channel_configure {
-	u8		cmd;
-	u8		device_num;
+	uint8_t		cmd;
+	uint8_t		device_num;
 	uint32_t	channel_num;
 };
 
 struct epm_psoc_set_avg {
-	u8	cmd;
-	u8	avg_period;
-	u8	return_code;
+	uint8_t	cmd;
+	uint8_t	avg_period;
+	uint8_t	return_code;
 };
 
 struct epm_psoc_get_data {
-	u8		cmd;
-	u8		dev_num;
-	u8		chan_num;
+	uint8_t		cmd;
+	uint8_t		dev_num;
+	uint8_t		chan_num;
 	uint32_t	timestamp_resp_value;
 	uint32_t	reading_value;
 };
 
 struct epm_psoc_get_buffered_data {
-	u8		cmd;
-	u8		dev_num;
-	u8		status_mask;
-	u8		chan_idx;
+	uint8_t		cmd;
+	uint8_t		dev_num;
+	uint8_t		status_mask;
+	uint8_t		chan_idx;
 	uint32_t	chan_mask;
 	uint32_t	timestamp_start;
 	uint32_t	timestamp_end;
-	u8		buff_data[48];
+	uint8_t		buff_data[48];
 };
 
 struct epm_psoc_system_time_stamp {
-	u8		cmd;
+	uint8_t		cmd;
 	uint32_t	timestamp;
 };
 
 struct epm_psoc_set_channel {
-	u8		cmd;
-	u8		dev_num;
+	uint8_t		cmd;
+	uint8_t		dev_num;
 	uint32_t	channel_mask;
 };
 
@@ -72,23 +71,23 @@
 };
 
 struct epm_psoc_get_avg_buffered_switch_data {
-	u8			cmd;
-	u8			status;
+	uint8_t			cmd;
+	uint8_t			status;
 	uint32_t		timestamp_start;
 	uint32_t		channel_mask;
-	u8			avg_data[54];
+	uint8_t			avg_data[54];
 	struct result_buffer	data[54];
 };
 
 struct epm_psoc_set_channel_switch {
-	u8		cmd;
-	u8		dev;
+	uint8_t		cmd;
+	uint8_t		dev;
 	uint32_t	delay;
 };
 
 struct epm_psoc_set_vadc {
-	u8		cmd;
-	u8		vadc_dev;
+	uint8_t		cmd;
+	uint8_t		vadc_dev;
 	uint32_t	vadc_voltage;
 };
 
@@ -97,6 +96,7 @@
 	uint32_t gain;
 };
 
+#ifdef __KERNEL__
 struct epm_adc_platform_data {
 	struct epm_chan_properties *channel;
 	uint32_t num_channels;
diff --git a/include/linux/iommu.h b/include/linux/iommu.h
index 95b15d6..d4fa3aa 100644
--- a/include/linux/iommu.h
+++ b/include/linux/iommu.h
@@ -22,12 +22,14 @@
 #include <linux/types.h>
 #include <linux/errno.h>
 #include <linux/scatterlist.h>
+#include <linux/err.h>
 
 #define IOMMU_READ	(1)
 #define IOMMU_WRITE	(2)
 #define IOMMU_CACHE	(4) /* DMA cache coherency */
 
 struct iommu_ops;
+struct iommu_group;
 struct bus_type;
 struct device;
 struct iommu_domain;
@@ -37,12 +39,13 @@
 #define IOMMU_FAULT_WRITE	0x1
 
 typedef int (*iommu_fault_handler_t)(struct iommu_domain *,
-				struct device *, unsigned long, int);
+			struct device *, unsigned long, int, void *);
 
 struct iommu_domain {
 	struct iommu_ops *ops;
 	void *priv;
 	iommu_fault_handler_t handler;
+	void *handler_token;
 };
 
 #define IOMMU_CAP_CACHE_COHERENCY	0x1
@@ -61,6 +64,8 @@
  * @iova_to_phys: translate iova to physical address
  * @domain_has_cap: domain capabilities query
  * @commit: commit iommu domain
+ * @add_device: add device to iommu grouping
+ * @remove_device: remove device from iommu grouping
  * @pgsize_bitmap: bitmap of supported page sizes
  */
 struct iommu_ops {
@@ -81,10 +86,18 @@
 	int (*domain_has_cap)(struct iommu_domain *domain,
 			      unsigned long cap);
 	phys_addr_t (*get_pt_base_addr)(struct iommu_domain *domain);
-	int (*device_group)(struct device *dev, unsigned int *groupid);
+	int (*add_device)(struct device *dev);
+	void (*remove_device)(struct device *dev);
 	unsigned long pgsize_bitmap;
 };
 
+#define IOMMU_GROUP_NOTIFY_ADD_DEVICE		1 /* Device added */
+#define IOMMU_GROUP_NOTIFY_DEL_DEVICE		2 /* Pre Device removed */
+#define IOMMU_GROUP_NOTIFY_BIND_DRIVER		3 /* Pre Driver bind */
+#define IOMMU_GROUP_NOTIFY_BOUND_DRIVER		4 /* Post Driver bind */
+#define IOMMU_GROUP_NOTIFY_UNBIND_DRIVER	5 /* Pre Driver unbind */
+#define IOMMU_GROUP_NOTIFY_UNBOUND_DRIVER	6 /* Post Driver unbind */
+
 extern int bus_set_iommu(struct bus_type *bus, struct iommu_ops *ops);
 extern bool iommu_present(struct bus_type *bus);
 extern struct iommu_domain *iommu_domain_alloc(struct bus_type *bus, int flags);
@@ -107,8 +120,30 @@
 				unsigned long cap);
 extern phys_addr_t iommu_get_pt_base_addr(struct iommu_domain *domain);
 extern void iommu_set_fault_handler(struct iommu_domain *domain,
-					iommu_fault_handler_t handler);
-extern int iommu_device_group(struct device *dev, unsigned int *groupid);
+			iommu_fault_handler_t handler, void *token);
+
+extern int iommu_attach_group(struct iommu_domain *domain,
+			      struct iommu_group *group);
+extern void iommu_detach_group(struct iommu_domain *domain,
+			       struct iommu_group *group);
+extern struct iommu_group *iommu_group_alloc(void);
+extern void *iommu_group_get_iommudata(struct iommu_group *group);
+extern void iommu_group_set_iommudata(struct iommu_group *group,
+				      void *iommu_data,
+				      void (*release)(void *iommu_data));
+extern int iommu_group_set_name(struct iommu_group *group, const char *name);
+extern int iommu_group_add_device(struct iommu_group *group,
+				  struct device *dev);
+extern void iommu_group_remove_device(struct device *dev);
+extern int iommu_group_for_each_dev(struct iommu_group *group, void *data,
+				    int (*fn)(struct device *, void *));
+extern struct iommu_group *iommu_group_get(struct device *dev);
+extern void iommu_group_put(struct iommu_group *group);
+extern int iommu_group_register_notifier(struct iommu_group *group,
+					 struct notifier_block *nb);
+extern int iommu_group_unregister_notifier(struct iommu_group *group,
+					   struct notifier_block *nb);
+extern int iommu_group_id(struct iommu_group *group);
 
 /**
  * report_iommu_fault() - report about an IOMMU fault to the IOMMU framework
@@ -144,7 +179,8 @@
 	 * invoke it.
 	 */
 	if (domain->handler)
-		ret = domain->handler(domain, dev, iova, flags);
+		ret = domain->handler(domain, dev, iova, flags,
+						domain->handler_token);
 
 	return ret;
 }
@@ -152,6 +188,7 @@
 #else /* CONFIG_IOMMU_API */
 
 struct iommu_ops {};
+struct iommu_group {};
 
 static inline bool iommu_present(struct bus_type *bus)
 {
@@ -221,15 +258,85 @@
 }
 
 static inline void iommu_set_fault_handler(struct iommu_domain *domain,
-					iommu_fault_handler_t handler)
+				iommu_fault_handler_t handler, void *token)
 {
 }
 
-static inline int iommu_device_group(struct device *dev, unsigned int *groupid)
+static inline int iommu_attach_group(struct iommu_domain *domain,
+				     struct iommu_group *group)
 {
 	return -ENODEV;
 }
 
+static inline void iommu_detach_group(struct iommu_domain *domain,
+				      struct iommu_group *group)
+{
+}
+
+static inline struct iommu_group *iommu_group_alloc(void)
+{
+	return ERR_PTR(-ENODEV);
+}
+
+static inline void *iommu_group_get_iommudata(struct iommu_group *group)
+{
+	return NULL;
+}
+
+static inline void iommu_group_set_iommudata(struct iommu_group *group,
+					     void *iommu_data,
+					     void (*release)(void *iommu_data))
+{
+}
+
+static inline int iommu_group_set_name(struct iommu_group *group,
+				       const char *name)
+{
+	return -ENODEV;
+}
+
+static inline int iommu_group_add_device(struct iommu_group *group,
+					 struct device *dev)
+{
+	return -ENODEV;
+}
+
+static inline void iommu_group_remove_device(struct device *dev)
+{
+}
+
+static inline int iommu_group_for_each_dev(struct iommu_group *group,
+					   void *data,
+					   int (*fn)(struct device *, void *))
+{
+	return -ENODEV;
+}
+
+static inline struct iommu_group *iommu_group_get(struct device *dev)
+{
+	return NULL;
+}
+
+static inline void iommu_group_put(struct iommu_group *group)
+{
+}
+
+static inline int iommu_group_register_notifier(struct iommu_group *group,
+				  struct notifier_block *nb)
+{
+	return -ENODEV;
+}
+
+static inline int iommu_group_unregister_notifier(struct iommu_group *group,
+				    struct notifier_block *nb)
+{
+	return 0;
+}
+
+static inline int iommu_group_id(struct iommu_group *group)
+{
+	return -ENODEV;
+}
 #endif /* CONFIG_IOMMU_API */
 
 #endif /* __LINUX_IOMMU_H */
diff --git a/include/linux/mfd/pm8xxx/pm8921-charger.h b/include/linux/mfd/pm8xxx/pm8921-charger.h
index ae2c3d8..44f8538 100644
--- a/include/linux/mfd/pm8xxx/pm8921-charger.h
+++ b/include/linux/mfd/pm8xxx/pm8921-charger.h
@@ -123,6 +123,21 @@
  *			resistance of the pads, connectors, battery terminals
  *			and rsense.
  * @led_src_config:	Power source for anode of charger indicator LED.
+ * @btc_override:	disable the comparators for conifugrations where a
+ *			suitable voltages don't appear on vbatt therm line
+ *			for the charger to detect battery is either cold / hot.
+ * @btc_override_cold_degc:	Temperature in degCelcius when the battery is
+ *				deemed cold and charging never happens. Used
+ *				only if btc_override = 1
+ * @btc_override_hot_degc:	Temperature in degCelcius when the battery is
+ *				deemed hot and charging never happens. Used
+ *				only if btc_override = 1
+ * @btc_delay_ms:	Delay in milliseconds to monitor the battery temperature
+ *			while charging when btc_override = 1
+ * @btc_panic_if_cant_stop_chg:	flag to instruct the driver to panic if the
+ *				driver couldn't stop charging when battery
+ *				temperature is out of bounds. Used only if
+ *				btc_override = 1
  */
 struct pm8921_charger_platform_data {
 	struct pm8xxx_charger_core_data	charger_cdata;
@@ -163,6 +178,11 @@
 	int				rconn_mohm;
 	enum pm8921_chg_led_src_config	led_src_config;
 	int				battery_less_hardware;
+	int				btc_override;
+	int				btc_override_cold_degc;
+	int				btc_override_hot_degc;
+	int				btc_delay_ms;
+	int				btc_panic_if_cant_stop_chg;
 };
 
 enum pm8921_charger_source {
diff --git a/include/linux/mfd/wcd9xxx/core.h b/include/linux/mfd/wcd9xxx/core.h
index 4e9e1ce..2874a3b 100644
--- a/include/linux/mfd/wcd9xxx/core.h
+++ b/include/linux/mfd/wcd9xxx/core.h
@@ -22,10 +22,6 @@
 #define WCD9XXX_NUM_IRQ_REGS 4
 
 #define WCD9XXX_SLIM_NUM_PORT_REG 3
-
-#define WCD9XXX_INTERFACE_TYPE_SLIMBUS	0x00
-#define WCD9XXX_INTERFACE_TYPE_I2C	0x01
-
 #define TABLA_VERSION_1_0	0
 #define TABLA_VERSION_1_1	1
 #define TABLA_VERSION_2_0	2
@@ -135,6 +131,12 @@
 	wait_queue_head_t dai_wait;
 };
 
+enum wcd9xxx_intf_status {
+	WCD9XXX_INTERFACE_TYPE_PROBING,
+	WCD9XXX_INTERFACE_TYPE_SLIMBUS,
+	WCD9XXX_INTERFACE_TYPE_I2C,
+};
+
 #define WCD9XXX_CH(xport, xshift) \
 	{.port = xport, .shift = xshift}
 
@@ -177,6 +179,7 @@
 	u32 num_tx_port;
 	struct wcd9xxx_ch *rx_chs;
 	struct wcd9xxx_ch *tx_chs;
+	u32 mclk_rate;
 };
 
 int wcd9xxx_reg_read(struct wcd9xxx *wcd9xxx, unsigned short reg);
@@ -192,7 +195,7 @@
 int wcd9xxx_irq_init(struct wcd9xxx *wcd9xxx);
 void wcd9xxx_irq_exit(struct wcd9xxx *wcd9xxx);
 int wcd9xxx_get_logical_addresses(u8 *pgd_la, u8 *inf_la);
-int wcd9xxx_get_intf_type(void);
+enum wcd9xxx_intf_status wcd9xxx_get_intf_type(void);
 
 bool wcd9xxx_lock_sleep(struct wcd9xxx *wcd9xxx);
 void wcd9xxx_unlock_sleep(struct wcd9xxx *wcd9xxx);
diff --git a/include/linux/mfd/wcd9xxx/pdata.h b/include/linux/mfd/wcd9xxx/pdata.h
index a7ca417..bfd95a6 100644
--- a/include/linux/mfd/wcd9xxx/pdata.h
+++ b/include/linux/mfd/wcd9xxx/pdata.h
@@ -154,6 +154,7 @@
 	struct wcd9xxx_micbias_setting micbias;
 	struct wcd9xxx_ocp_setting ocp;
 	struct wcd9xxx_regulator regulator[MAX_REGULATOR];
+	u32 mclk_rate;
 };
 
 #endif
diff --git a/include/linux/mmc/core.h b/include/linux/mmc/core.h
index 7247696..24b9790 100644
--- a/include/linux/mmc/core.h
+++ b/include/linux/mmc/core.h
@@ -178,6 +178,8 @@
 extern unsigned int mmc_calc_max_discard(struct mmc_card *card);
 
 extern int mmc_set_blocklen(struct mmc_card *card, unsigned int blocklen);
+extern int mmc_set_blockcount(struct mmc_card *card, unsigned int blockcount,
+			      bool is_rel_write);
 extern int mmc_hw_reset(struct mmc_host *host);
 extern int mmc_hw_reset_check(struct mmc_host *host);
 extern int mmc_can_reset(struct mmc_card *card);
diff --git a/include/linux/mmzone.h b/include/linux/mmzone.h
index f8a3a10..7bdd3f2 100644
--- a/include/linux/mmzone.h
+++ b/include/linux/mmzone.h
@@ -62,9 +62,11 @@
 };
 
 #ifdef CONFIG_CMA
+bool is_cma_pageblock(struct page *page);
 #  define is_migrate_cma(migratetype) unlikely((migratetype) == MIGRATE_CMA)
 #  define cma_wmark_pages(zone)	zone->min_cma_pages
 #else
+#  define is_cma_pageblock(page) false
 #  define is_migrate_cma(migratetype) false
 #  define cma_wmark_pages(zone) 0
 #endif
diff --git a/include/linux/msm_ion.h b/include/linux/msm_ion.h
index 21648ad..d423b26 100644
--- a/include/linux/msm_ion.h
+++ b/include/linux/msm_ion.h
@@ -137,6 +137,7 @@
  *			goes from 1 -> 0
  * @setup_region:	function to be called upon ion registration
  * @memory_type:Memory type used for the heap
+ * @no_nonsecure_alloc: don't allow non-secure allocations from this heap
  *
  */
 struct ion_cp_heap_pdata {
@@ -155,6 +156,7 @@
 	int (*release_region)(void *);
 	void *(*setup_region)(void);
 	enum ion_memory_types memory_type;
+	int no_nonsecure_alloc;
 };
 
 /**
diff --git a/include/linux/netfilter_ipv4/Kbuild b/include/linux/netfilter_ipv4/Kbuild
index 31f8bec..6733ccc 100644
--- a/include/linux/netfilter_ipv4/Kbuild
+++ b/include/linux/netfilter_ipv4/Kbuild
@@ -10,3 +10,4 @@
 header-y += ipt_ah.h
 header-y += ipt_ecn.h
 header-y += ipt_ttl.h
+header-y += ipt_NATTYPE.h
diff --git a/include/linux/netfilter_ipv4/ipt_NATTYPE.h b/include/linux/netfilter_ipv4/ipt_NATTYPE.h
new file mode 100644
index 0000000..b612290
--- /dev/null
+++ b/include/linux/netfilter_ipv4/ipt_NATTYPE.h
@@ -0,0 +1,25 @@
+#ifndef _IPT_NATTYPE_H_target
+#define _IPT_NATTYPE_H_target
+
+#define NATTYPE_TIMEOUT 300
+
+enum nattype_mode {
+	MODE_DNAT,
+	MODE_FORWARD_IN,
+	MODE_FORWARD_OUT
+};
+
+enum nattype_type {
+	TYPE_PORT_ADDRESS_RESTRICTED,
+	TYPE_ENDPOINT_INDEPENDENT,
+	TYPE_ADDRESS_RESTRICTED
+};
+
+
+struct ipt_nattype_info {
+	u_int16_t mode;
+	u_int16_t type;
+};
+
+#endif /*_IPT_NATTYPE_H_target*/
+
diff --git a/include/linux/qpnp/qpnp-adc.h b/include/linux/qpnp/qpnp-adc.h
index fc34b22..3ab7b9d 100644
--- a/include/linux/qpnp/qpnp-adc.h
+++ b/include/linux/qpnp/qpnp-adc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012, Code Aurora Forum. All rights reserved.
+ * Copyright (c) 2012, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -660,13 +660,16 @@
  *		thresholds.
  * @timer_interval: Select polling rate from qpnp_adc_meas_timer_1 type.
  * @threshold_notification: Notification callback once threshold are crossed.
+ * @usbid_ctx: A context of void type.
  */
 struct qpnp_adc_tm_usbid_param {
 	int32_t					high_thr;
 	int32_t					low_thr;
 	enum qpnp_state_request			state_request;
 	enum qpnp_adc_meas_timer_1		timer_interval;
-	void	(*threshold_notification) (enum qpnp_tm_state state);
+	void					*usbid_ctx;
+	void	(*threshold_notification) (enum qpnp_tm_state state,
+				void *ctx);
 };
 
 /**
@@ -743,16 +746,32 @@
  *			   input channel.
  * @offset_gain_denominator: The inverse denominator of the gain applied to the
  *			     input channel.
+ * @high_thr: High threshold voltage that is requested to be set.
+ * @low_thr: Low threshold voltage that is requested to be set.
+ * @timer_select: Choosen from one of the 3 timers to set the polling rate for
+ *		  the VADC_BTM channel.
+ * @meas_interval1: Polling rate to set for timer 1.
+ * @meas_interval2: Polling rate to set for timer 2.
+ * @tm_channel_select: BTM channel number for the 5 VADC_BTM channels.
+ * @state_request: User can select either enable or disable high/low or both
+ * activation levels based on the qpnp_state_request type.
  * @adc_graph: ADC graph for the channel of struct type qpnp_adc_linear_graph.
  */
 struct qpnp_vadc_chan_properties {
 	uint32_t			offset_gain_numerator;
 	uint32_t			offset_gain_denominator;
+	uint32_t				high_thr;
+	uint32_t				low_thr;
+	enum qpnp_adc_meas_timer_select		timer_select;
+	enum qpnp_adc_meas_timer_1		meas_interval1;
+	enum qpnp_adc_meas_timer_2		meas_interval2;
+	enum qpnp_adc_tm_channel_select		tm_channel_select;
+	enum qpnp_state_request			state_request;
 	struct qpnp_vadc_linear_graph	adc_graph[2];
 };
 
 /**
- * struct qpnp_adc_result - Represent the result of the QPNP ADC.
+ * struct qpnp_vadc_result - Represent the result of the QPNP ADC.
  * @chan: The channel number of the requested conversion.
  * @adc_code: The pre-calibrated digital output of a given ADC relative to the
  *	      the ADC reference.
@@ -774,7 +793,7 @@
 };
 
 /**
- * struct qpnp_vadc_amux - AMUX properties for individual channel
+ * struct qpnp_adc_amux - AMUX properties for individual channel
  * @name: Channel string name.
  * @channel_num: Channel in integer used from qpnp_adc_channels.
  * @chan_path_prescaling: Channel scaling performed on the input signal.
@@ -783,7 +802,7 @@
  *		 each individual channel whether it is voltage, current,
  *		 temperature, etc and compensates the channel properties.
  */
-struct qpnp_vadc_amux {
+struct qpnp_adc_amux {
 	char					*name;
 	enum qpnp_vadc_channels			channel_num;
 	enum qpnp_adc_channel_scaling_param	chan_path_prescaling;
@@ -858,6 +877,11 @@
  * @amux_prop - AMUX properties representing the ADC peripheral.
  * @adc_channels - ADC channel properties for the ADC peripheral.
  * @adc_irq_eoc - End of Conversion IRQ.
+ * @adc_irq_fifo_not_empty - Conversion sequencer request written
+ *			to FIFO when not empty.
+ * @adc_irq_conv_seq_timeout - Conversion sequencer trigger timeout.
+ * @adc_high_thr_irq - Output higher than high threshold set for measurement.
+ * @adc_low_thr_irq - Output lower than low threshold set for measurement.
  * @adc_lock - ADC lock for access to the peripheral.
  * @adc_rslt_completion - ADC result notification after interrupt
  *			  is received.
@@ -869,8 +893,12 @@
 	uint16_t			offset;
 	struct qpnp_adc_properties	*adc_prop;
 	struct qpnp_adc_amux_properties	*amux_prop;
-	struct qpnp_vadc_amux		*adc_channels;
+	struct qpnp_adc_amux		*adc_channels;
 	int				adc_irq_eoc;
+	int				adc_irq_fifo_not_empty;
+	int				adc_irq_conv_seq_timeout;
+	int				adc_high_thr_irq;
+	int				adc_low_thr_irq;
 	struct mutex			adc_lock;
 	struct completion		adc_rslt_completion;
 	struct qpnp_iadc_calib		calib;
@@ -937,13 +965,6 @@
 					struct qpnp_adc_drv *adc_qpnp);
 
 /**
- * qpnp_vadc_configure() - Configure ADC device to start conversion.
- * @chan_prop:	Individual channel properties for the AMUX channel.
- */
-int32_t qpnp_vadc_configure(
-			struct qpnp_adc_amux_properties *chan_prop);
-
-/**
  * qpnp_adc_scale_default() - Scales the pre-calibrated digital output
  *		of an ADC to the ADC reference and compensates for the
  *		gain and offset.
@@ -1234,4 +1255,65 @@
 { return -ENXIO; }
 #endif
 
+/* Public API */
+#if defined(CONFIG_THERMAL_QPNP_ADC_TM)				\
+			|| defined(CONFIG_THERMAL_QPNP_ADC_TM_MODULE)
+/**
+ * qpnp_adc_tm_usbid_configure() - Configures Channel 0 of VADC_BTM to
+ *		monitor USB_ID channel using 100k internal pull-up.
+ *		USB driver passes the high/low voltage threshold along
+ *		with the notification callback once the set thresholds
+ *		are crossed.
+ * @param:	Structure pointer of qpnp_adc_tm_usbid_param type.
+ *		Clients pass the low/high voltage along with the threshold
+ *		notification callback.
+ */
+int32_t qpnp_adc_tm_usbid_configure(struct qpnp_adc_tm_usbid_param *param);
+/**
+ * qpnp_adc_tm_usbid_end() - Disables the monitoring of channel 0 thats
+ *		assigned for monitoring USB_ID. Disables the low/high
+ *		threshold activation for channel 0 as well.
+ * @param:	none.
+ */
+int32_t qpnp_adc_tm_usbid_end(void);
+/**
+ * qpnp_adc_tm_usbid_configure() - Configures Channel 1 of VADC_BTM to
+ *		monitor batt_therm channel using 100k internal pull-up.
+ *		Battery driver passes the high/low voltage threshold along
+ *		with the notification callback once the set thresholds
+ *		are crossed.
+ * @param:	Structure pointer of qpnp_adc_tm_btm_param type.
+ *		Clients pass the low/high temperature along with the threshold
+ *		notification callback.
+ */
+int32_t qpnp_adc_tm_btm_configure(struct qpnp_adc_tm_btm_param *param);
+/**
+ * qpnp_adc_tm_btm_end() - Disables the monitoring of channel 1 thats
+ *		assigned for monitoring batt_therm. Disables the low/high
+ *		threshold activation for channel 1 as well.
+ * @param:	none.
+ */
+int32_t qpnp_adc_tm_btm_end(void);
+/**
+ * qpnp_adc_tm_is_ready() - Clients can use this API to check if the
+ *			  device is ready to use.
+ * @result:	0 on success and -EPROBE_DEFER when probe for the device
+ *		has not occured.
+ */
+int32_t	qpnp_adc_tm_is_ready(void);
+#else
+static inline int32_t qpnp_adc_tm_usbid_configure(
+			struct qpnp_adc_tm_usbid_param *param)
+{ return -ENXIO; }
+static inline int32_t qpnp_adc_tm_usbid_end(void)
+{ return -ENXIO; }
+static inline int32_t qpnp_adc_tm_btm_configure(
+		struct qpnp_adc_tm_btm_param *param)
+{ return -ENXIO; }
+static inline int32_t qpnp_adc_tm_btm_end(void)
+{ return -ENXIO; }
+static inline int32_t qpnp_adc_tm_is_ready(void)
+{ return -ENXIO; }
+#endif
+
 #endif
diff --git a/include/linux/regulator/krait-regulator.h b/include/linux/regulator/krait-regulator.h
index eb1c3fd..9806225 100644
--- a/include/linux/regulator/krait-regulator.h
+++ b/include/linux/regulator/krait-regulator.h
@@ -22,7 +22,17 @@
  * should be called from appropriate initialization code. Returns 0 on
  * success and error on failure.
  */
+
+#ifdef CONFIG_ARCH_MSM8974
 int __init krait_power_init(void);
 void secondary_cpu_hs_init(void *base_ptr);
+#else
+static inline int __init krait_power_init(void)
+{
+	return -ENOSYS;
+}
+
+static inline void secondary_cpu_hs_init(void *base_ptr) {}
+#endif
 
 #endif
diff --git a/include/linux/smsc3503.h b/include/linux/smsc3503.h
index 66ba003..857ad1f 100644
--- a/include/linux/smsc3503.h
+++ b/include/linux/smsc3503.h
@@ -43,6 +43,8 @@
 
 struct smsc_hub_platform_data {
 	unsigned hub_reset;
+	unsigned refclk_gpio;
+	unsigned int_gpio;
 };
 
 #endif
diff --git a/include/linux/usb/msm_hsusb.h b/include/linux/usb/msm_hsusb.h
index 5b7820d..bcbdec4 100644
--- a/include/linux/usb/msm_hsusb.h
+++ b/include/linux/usb/msm_hsusb.h
@@ -394,6 +394,10 @@
 struct msm_hsic_host_platform_data {
 	unsigned strobe;
 	unsigned data;
+	bool ignore_cal_pad_config;
+	int strobe_pad_offset;
+	int data_pad_offset;
+
 	struct msm_bus_scale_pdata *bus_scale_table;
 	unsigned log2_irq_thresh;
 
@@ -479,12 +483,35 @@
 	MAX_BAMS,
 };
 
+/**
+ * struct usb_ext_notification: event notification structure
+ * @notify: pointer to client function to call when ID event is detected.
+ *          The last parameter is provided by driver to be called back when
+ *          external client indicates it is done using the USB. This function
+ *          should return 0 if handled successfully, otherise an error code.
+ * @ctxt: client-specific context pointer
+ *
+ * This structure should be used by clients wishing to register (via
+ * msm_register_usb_ext_notification) for event notification whenever a USB
+ * cable is plugged in and ID pin status changes. Clients must provide a
+ * callback function pointer. If this callback returns 0, the USB driver will
+ * assume the client is "taking over" the connection, and will relinquish any
+ * further processing until its callback (passed via the third parameter) is
+ * called with the online parameter set to false.
+ */
+struct usb_ext_notification {
+	int (*notify)(void *, int, void (*)(int online));
+	void *ctxt;
+};
+
 #ifdef CONFIG_USB_DWC3_MSM
 int msm_ep_config(struct usb_ep *ep);
 int msm_ep_unconfig(struct usb_ep *ep);
 int msm_data_fifo_config(struct usb_ep *ep, u32 addr, u32 size,
 	u8 dst_pipe_idx);
 
+int msm_register_usb_ext_notification(struct usb_ext_notification *info);
+
 #else
 static inline int msm_data_fifo_config(struct usb_ep *ep, u32 addr, u32 size,
 	u8 dst_pipe_idx)
@@ -501,5 +528,11 @@
 {
 	return -ENODEV;
 }
+
+static inline int msm_register_usb_ext_notification(
+					struct usb_ext_notification *info)
+{
+	return -ENODEV;
+}
 #endif
 #endif
diff --git a/include/linux/videodev2.h b/include/linux/videodev2.h
index e5e0bb4..bc25e24 100644
--- a/include/linux/videodev2.h
+++ b/include/linux/videodev2.h
@@ -1840,6 +1840,12 @@
 	V4L2_MPEG_VIDC_VIDEO_H264_VUI_TIMING_INFO_ENABLED = 1
 };
 
+#define V4L2_CID_MPEG_VIDC_SET_PERF_LEVEL (V4L2_CID_MPEG_MSM_VIDC_BASE + 26)
+enum v4l2_mpeg_vidc_perf_level {
+	V4L2_CID_MPEG_VIDC_PERF_LEVEL_PERFORMANCE		= 0,
+	V4L2_CID_MPEG_VIDC_PERF_LEVEL_TURBO			= 1,
+};
+
 /*  Camera class control IDs */
 #define V4L2_CID_CAMERA_CLASS_BASE 	(V4L2_CTRL_CLASS_CAMERA | 0x900)
 #define V4L2_CID_CAMERA_CLASS 		(V4L2_CTRL_CLASS_CAMERA | 1)
@@ -2133,6 +2139,7 @@
 };
 
 #define V4L2_QCOM_BUF_FLAG_CODECCONFIG	0x4000
+#define V4L2_QCOM_BUF_FLAG_EOSEQ  0x8000
 
 /* Decoder commands */
 #define V4L2_DEC_CMD_START       (0)
diff --git a/include/media/Kbuild b/include/media/Kbuild
index 70f6334..fc764eb 100644
--- a/include/media/Kbuild
+++ b/include/media/Kbuild
@@ -8,3 +8,4 @@
 header-y += msm_mercury.h
 header-y += msm_jpeg.h
 header-y += msm_media_info.h
+header-y += msm_vidc.h
diff --git a/include/media/msm_media_info.h b/include/media/msm_media_info.h
index 13ce043..ab76d79 100644
--- a/include/media/msm_media_info.h
+++ b/include/media/msm_media_info.h
@@ -97,7 +97,7 @@
 	uv_sclines = VENUS_UV_SCANLINES(color_fmt, height);
 	switch (color_fmt) {
 	case COLOR_FMT_NV12:
-		uv_alignment = 0;
+		uv_alignment = 4096;
 		y_plane = y_stride * y_sclines;
 		uv_plane = uv_stride * uv_sclines + uv_alignment;
 		size = y_plane + uv_plane;
diff --git a/include/media/msm_vidc.h b/include/media/msm_vidc.h
index 0fd11a3..4261d34 100644
--- a/include/media/msm_vidc.h
+++ b/include/media/msm_vidc.h
@@ -1,19 +1,8 @@
-/* Copyright (c) 2012, Code Aurora Forum. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 and
- * only version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- */
-
 #ifndef _MSM_VIDC_H_
 #define _MSM_VIDC_H_
 
+#ifdef __KERNEL__
+
 #include <linux/poll.h>
 #include <linux/videodev2.h>
 
@@ -71,3 +60,59 @@
 int msm_vidc_wait(void *instance);
 int msm_vidc_s_parm(void *instance, struct v4l2_streamparm *a);
 #endif
+struct msm_vidc_interlace_payload {
+	unsigned int format;
+};
+struct msm_vidc_framerate_payload {
+	unsigned int frame_rate;
+};
+struct msm_vidc_ts_payload {
+	unsigned int timestamp_hi;
+	unsigned int timestamp_lo;
+};
+struct msm_vidc_concealmb_payload {
+	unsigned int num_mbs;
+};
+struct msm_vidc_recoverysei_payload {
+	unsigned int flags;
+};
+struct msm_vidc_panscan_window {
+	unsigned int panscan_height_offset;
+	unsigned int panscan_width_offset;
+	unsigned int panscan_window_width;
+	unsigned int panscan_window_height;
+};
+struct msm_vidc_panscan_window_payload {
+	unsigned int num_panscan_windows;
+	struct msm_vidc_panscan_window wnd[1];
+};
+enum msm_vidc_extradata_type {
+	EXTRADATA_NONE = 0x00000000,
+	EXTRADATA_MB_QUANTIZATION = 0x00000001,
+	EXTRADATA_INTERLACE_VIDEO = 0x00000002,
+	EXTRADATA_VC1_FRAMEDISP = 0x00000003,
+	EXTRADATA_VC1_SEQDISP = 0x00000004,
+	EXTRADATA_TIMESTAMP = 0x00000005,
+	EXTRADATA_S3D_FRAME_PACKING = 0x00000006,
+	EXTRADATA_FRAME_RATE = 0x00000007,
+	EXTRADATA_PANSCAN_WINDOW = 0x00000008,
+	EXTRADATA_RECOVERY_POINT_SEI = 0x00000009,
+	EXTRADATA_MULTISLICE_INFO = 0x7F100000,
+	EXTRADATA_NUM_CONCEALED_MB = 0x7F100001,
+	EXTRADATA_INDEX = 0x7F100002,
+	EXTRADATA_METADATA_FILLER = 0x7FE00002,
+};
+enum msm_vidc_interlace_type {
+	INTERLACE_FRAME_PROGRESSIVE = 0x01,
+	INTERLACE_INTERLEAVE_FRAME_TOPFIELDFIRST = 0x02,
+	INTERLACE_INTERLEAVE_FRAME_BOTTOMFIELDFIRST = 0x04,
+	INTERLACE_FRAME_TOPFIELDFIRST = 0x08,
+	INTERLACE_FRAME_BOTTOMFIELDFIRST = 0x10,
+};
+enum msm_vidc_recovery_sei {
+	FRAME_RECONSTRUCTION_INCORRECT = 0x0,
+	FRAME_RECONSTRUCTION_CORRECT = 0x01,
+	FRAME_RECONSTRUCTION_APPROXIMATELY_CORRECT = 0x02,
+};
+
+#endif
diff --git a/include/media/vcap_fmt.h b/include/media/vcap_fmt.h
index 369cf45..2787e8d 100644
--- a/include/media/vcap_fmt.h
+++ b/include/media/vcap_fmt.h
@@ -47,6 +47,11 @@
 	HAL_VCAP_RGB,
 };
 
+enum nr_threshold_mode {
+	NR_THRESHOLD_STATIC = 0,
+	NR_THRESHOLD_DYNAMIC,
+};
+
 enum nr_mode {
 	NR_DISABLE = 0,
 	NR_AUTO,
@@ -73,6 +78,7 @@
 };
 
 struct nr_param {
+	enum nr_threshold_mode threshold;
 	enum nr_mode mode;
 	enum nr_decay_ratio decay_ratio;
 	uint8_t window;
diff --git a/include/sound/apr_audio-v2.h b/include/sound/apr_audio-v2.h
index 3dd0ccd..88acdfc 100644
--- a/include/sound/apr_audio-v2.h
+++ b/include/sound/apr_audio-v2.h
@@ -281,6 +281,26 @@
 	 */
 } __packed;
 
+/* Defined specifically for in-band use, includes params */
+struct adm_cmd_set_pp_params_inband_v5 {
+	struct apr_hdr hdr;
+	/* LSW of parameter data payload address.*/
+	u32		payload_addr_lsw;
+	/* MSW of parameter data payload address.*/
+	u32		payload_addr_msw;
+	/* Memory map handle returned by ADM_CMD_SHARED_MEM_MAP_REGIONS */
+	/* command. If mem_map_handle is zero implies the message is in */
+	/* the payload */
+	u32		mem_map_handle;
+	/* Size in bytes of the variable payload accompanying this */
+	/* message or in shared memory. This is used for parsing the */
+	/* parameter payload. */
+	u32		payload_size;
+	/* Parameters passed for in band payload */
+	struct adm_param_data_v5	params;
+} __packed;
+
+
 /* Returns the status and COPP ID to an #ADM_CMD_DEVICE_OPEN_V5 command.
  */
 #define ADM_CMDRSP_DEVICE_OPEN_V5                      0x00010329
@@ -1884,6 +1904,67 @@
 	/* For 32 bit alignment. */
 } __packed;
 
+
+/* This param id is used to configure the Pseudoport interface */
+
+#define AFE_PARAM_ID_PSEUDO_PORT_CONFIG	0x00010219
+
+/* Version information used to handle future additions to the configuration
+ * interface (for backward compatibility).
+ */
+#define AFE_API_VERSION_PSEUDO_PORT_CONFIG                          0x1
+
+/* Enumeration for setting the timing_mode parameter to faster than real
+ * time.
+ */
+#define AFE_PSEUDOPORT_TIMING_MODE_FTRT                             0x0
+
+/* Enumeration for setting the timing_mode parameter to real time using
+ * timers.
+ */
+#define AFE_PSEUDOPORT_TIMING_MODE_TIMER                            0x1
+
+/* Payload of the AFE_PARAM_ID_PSEUDO_PORT_CONFIG parameter used by
+    AFE_MODULE_AUDIO_DEV_INTERFACE.
+*/
+struct afe_param_id_pseudo_port_cfg {
+	u32                  pseud_port_cfg_minor_version;
+	/*
+	 * Minor version used for tracking the version of the pseudoport
+	 * configuration interface.
+	 */
+
+	u16                  bit_width;
+	/* Bit width of the sample at values 16, 24 */
+
+	u16                  num_channels;
+	/* Number of channels at values  1 to 8 */
+
+	u16                  data_format;
+	/* Non-linear data format supported by the pseudoport (for future use).
+	 * At values #AFE_LINEAR_PCM_DATA
+	 */
+
+	u16                  timing_mode;
+	/* Indicates whether the pseudoport synchronizes to the clock or
+	 * operates faster than real time.
+	 * at values
+	 * - #AFE_PSEUDOPORT_TIMING_MODE_FTRT
+	 * - #AFE_PSEUDOPORT_TIMING_MODE_TIMER @tablebulletend
+	 */
+
+	u32                  sample_rate;
+	/* Sample rate at which the pseudoport will run.
+	 * at values
+	 * - #AFE_PORT_SAMPLE_RATE_8K
+	 * - #AFE_PORT_SAMPLE_RATE_32K
+	 * - #AFE_PORT_SAMPLE_RATE_48K
+	 * - #AFE_PORT_SAMPLE_RATE_96K
+	 * - #AFE_PORT_SAMPLE_RATE_192K @tablebulletend
+	 */
+} __packed;
+
+
 union afe_port_config {
 	struct afe_param_id_pcm_cfg               pcm;
 	struct afe_param_id_i2s_cfg               i2s;
@@ -1891,6 +1972,12 @@
 	struct afe_param_id_slimbus_cfg           slim_sch;
 	struct afe_param_id_rt_proxy_port_cfg     rtproxy;
 	struct afe_param_id_internal_bt_fm_cfg    int_bt_fm;
+	struct afe_param_id_pseudo_port_cfg       pseudo_port;
+} __packed;
+
+struct afe_audioif_config_command_no_payload {
+	struct apr_hdr			hdr;
+	struct afe_port_cmd_set_param_v2 param;
 } __packed;
 
 struct afe_audioif_config_command {
@@ -6129,6 +6216,111 @@
 #define VOICE_EVT_GET_PARAM_ACK				0x00011008
 
 
+/* SRS TRUMEDIA start */
+/* topology */
+#define SRS_TRUMEDIA_TOPOLOGY_ID			0x00010D90
+/* module */
+#define SRS_TRUMEDIA_MODULE_ID				0x10005010
+/* parameters */
+#define SRS_TRUMEDIA_PARAMS				0x10005011
+#define SRS_TRUMEDIA_PARAMS_WOWHD			0x10005012
+#define SRS_TRUMEDIA_PARAMS_CSHP			0x10005013
+#define SRS_TRUMEDIA_PARAMS_HPF				0x10005014
+#define SRS_TRUMEDIA_PARAMS_PEQ				0x10005015
+#define SRS_TRUMEDIA_PARAMS_HL				0x10005016
+
+#define SRS_ID_GLOBAL	0x00000001
+#define SRS_ID_WOWHD	0x00000002
+#define SRS_ID_CSHP	0x00000003
+#define SRS_ID_HPF	0x00000004
+#define SRS_ID_PEQ	0x00000005
+#define SRS_ID_HL	0x00000006
+
+#define SRS_CMD_UPLOAD		0x7FFF0000
+#define SRS_PARAM_INDEX_MASK	0x80000000
+#define SRS_PARAM_OFFSET_MASK	0x3FFF0000
+#define SRS_PARAM_VALUE_MASK	0x0000FFFF
+
+struct srs_trumedia_params_GLOBAL {
+	uint8_t                  v1;
+	uint8_t                  v2;
+	uint8_t                  v3;
+	uint8_t                  v4;
+	uint8_t                  v5;
+	uint8_t                  v6;
+	uint8_t                  v7;
+	uint8_t                  v8;
+} __packed;
+
+struct srs_trumedia_params_WOWHD {
+	uint32_t				v1;
+	uint16_t				v2;
+	uint16_t				v3;
+	uint16_t				v4;
+	uint16_t				v5;
+	uint16_t				v6;
+	uint16_t				v7;
+	uint16_t				v8;
+	uint16_t				v____A1;
+	uint32_t				v9;
+	uint16_t				v10;
+	uint16_t				v11;
+	uint32_t				v12[16];
+} __packed;
+
+struct srs_trumedia_params_CSHP {
+	uint32_t				v1;
+	uint16_t				v2;
+	uint16_t				v3;
+	uint16_t				v4;
+	uint16_t				v5;
+	uint16_t				v6;
+	uint16_t				v____A1;
+	uint32_t				v7;
+	uint16_t				v8;
+	uint16_t				v9;
+	uint32_t				v10[16];
+} __packed;
+
+struct srs_trumedia_params_HPF {
+	uint32_t				v1;
+	uint32_t				v2[26];
+} __packed;
+
+struct srs_trumedia_params_PEQ {
+	uint32_t				v1;
+	uint16_t				v2;
+	uint16_t				v3;
+	uint16_t				v4;
+	uint16_t				v____A1;
+	uint32_t				v5[26];
+	uint32_t				v6[26];
+} __packed;
+
+struct srs_trumedia_params_HL {
+	uint16_t				v1;
+	uint16_t				v2;
+	uint16_t				v3;
+	uint16_t				v____A1;
+	int32_t					v4;
+	uint32_t				v5;
+	uint16_t				v6;
+	uint16_t				v____A2;
+	uint32_t				v7;
+} __packed;
+
+struct srs_trumedia_params {
+	struct srs_trumedia_params_GLOBAL	global;
+	struct srs_trumedia_params_WOWHD	wowhd;
+	struct srs_trumedia_params_CSHP		cshp;
+	struct srs_trumedia_params_HPF		hpf;
+	struct srs_trumedia_params_PEQ		peq;
+	struct srs_trumedia_params_HL		hl;
+} __packed;
+/* SRS TruMedia end */
+
+
+
 /* ERROR CODES */
 /* Success. The operation completed with no errors. */
 #define ADSP_EOK          0x00000000
diff --git a/include/sound/apr_audio.h b/include/sound/apr_audio.h
index bfd7208..5afbfad 100644
--- a/include/sound/apr_audio.h
+++ b/include/sound/apr_audio.h
@@ -697,18 +697,6 @@
 #define ASM_OPEN_READ_PERF_MODE_BIT			(1<<29)
 #define ADM_MULTI_CH_COPP_OPEN_PERF_MODE_BIT		(1<<13)
 
-/* SRS TRUMEDIA GUIDS */
-/* topology */
-#define SRS_TRUMEDIA_TOPOLOGY_ID			0x00010D90
-/* module */
-#define SRS_TRUMEDIA_MODULE_ID				0x10005010
-/* parameters */
-#define SRS_TRUMEDIA_PARAMS				0x10005011
-#define SRS_TRUMEDIA_PARAMS_WOWHD			0x10005012
-#define SRS_TRUMEDIA_PARAMS_CSHP			0x10005013
-#define SRS_TRUMEDIA_PARAMS_HPF				0x10005014
-#define SRS_TRUMEDIA_PARAMS_PEQ				0x10005015
-#define SRS_TRUMEDIA_PARAMS_HL				0x10005016
 
 #define ASM_MAX_EQ_BANDS 12
 
@@ -1767,18 +1755,36 @@
 #define ADSP_ENOTIMPL     0x00000011 /* Operation is not implemented. */
 #define ADSP_ENEEDMORE    0x00000012 /* Operation needs more data or resources*/
 
-/* SRS TRUMEDIA start */
-#define SRS_ID_GLOBAL	0x00000001
-#define SRS_ID_WOWHD	0x00000002
-#define SRS_ID_CSHP	0x00000003
-#define SRS_ID_HPF	0x00000004
-#define SRS_ID_PEQ	0x00000005
-#define SRS_ID_HL	0x00000006
+/* SRS TRUMEDIA GUIDS */
+#define SRS_TRUMEDIA_TOPOLOGY_ID    0x00010D90
+#define SRS_TRUMEDIA_MODULE_ID      0x10005010
+#define SRS_TRUMEDIA_PARAMS         0x10005011
+#define SRS_TRUMEDIA_PARAMS_WOWHD   0x10005012
+#define SRS_TRUMEDIA_PARAMS_CSHP    0x10005013
+#define SRS_TRUMEDIA_PARAMS_HPF     0x10005014
+#define SRS_TRUMEDIA_PARAMS_PEQ     0x10005015
+#define SRS_TRUMEDIA_PARAMS_HL      0x10005016
 
-#define SRS_CMD_UPLOAD		0x7FFF0000
-#define SRS_PARAM_INDEX_MASK	0x80000000
-#define SRS_PARAM_OFFSET_MASK	0x3FFF0000
-#define SRS_PARAM_VALUE_MASK	0x0000FFFF
+/* SRS STUDIO SOUND 3D GUIDS */
+#define SRS_SS3D_TOPOLOGY_ID        0x00010720
+#define SRS_SS3D_MODULE_ID          0x10005020
+#define SRS_SS3D_PARAMS             0x10005021
+#define SRS_SS3D_PARAMS_CTRL        0x10005022
+#define SRS_SS3D_PARAMS_FILTER      0x10005023
+
+/* SRS ALSA CMD MASKS */
+#define SRS_CMD_UPLOAD              0x7FFF0000
+#define SRS_PARAM_INDEX_MASK        0x80000000
+#define SRS_PARAM_OFFSET_MASK       0x3FFF0000
+#define SRS_PARAM_VALUE_MASK        0x0000FFFF
+
+/* SRS TRUMEDIA start */
+#define SRS_ID_GLOBAL               0x00000001
+#define SRS_ID_WOWHD                0x00000002
+#define SRS_ID_CSHP                 0x00000003
+#define SRS_ID_HPF                  0x00000004
+#define SRS_ID_PEQ                  0x00000005
+#define SRS_ID_HL                   0x00000006
 
 struct srs_trumedia_params_GLOBAL {
 	uint8_t                  v1;
@@ -1856,7 +1862,41 @@
 	struct srs_trumedia_params_PEQ		peq;
 	struct srs_trumedia_params_HL		hl;
 } __packed;
+
 int srs_trumedia_open(int port_id, int srs_tech_id, void *srs_params);
 /* SRS TruMedia end */
 
+/* SRS Studio Sound 3D start */
+#define SRS_ID_SS3D_GLOBAL	0x00000001
+#define SRS_ID_SS3D_CTRL	0x00000002
+#define SRS_ID_SS3D_FILTER	0x00000003
+
+struct srs_SS3D_params_GLOBAL {
+	uint8_t                  v1;
+	uint8_t                  v2;
+	uint8_t                  v3;
+	uint8_t                  v4;
+	uint8_t                  v5;
+	uint8_t                  v6;
+	uint8_t                  v7;
+	uint8_t                  v8;
+} __packed;
+
+struct srs_SS3D_ctrl_params {
+	uint8_t				v[236];
+} __packed;
+
+struct srs_SS3D_filter_params {
+	uint8_t				v[28 + 2752];
+} __packed;
+
+struct srs_SS3D_params {
+	struct srs_SS3D_params_GLOBAL   global;
+	struct srs_SS3D_ctrl_params     ss3d;
+	struct srs_SS3D_filter_params   ss3d_f;
+} __packed;
+
+int srs_ss3d_open(int port_id, int srs_tech_id, void *srs_params);
+/* SRS Studio Sound 3D end */
+
 #endif /*_APR_AUDIO_H_*/
diff --git a/include/sound/q6adm-v2.h b/include/sound/q6adm-v2.h
index 9c43d09..fdc3cb9 100644
--- a/include/sound/q6adm-v2.h
+++ b/include/sound/q6adm-v2.h
@@ -27,6 +27,8 @@
 	unsigned int session_id;
 };
 
+int srs_trumedia_open(int port_id, int srs_tech_id, void *srs_params);
+
 int adm_open(int port, int path, int rate, int mode, int topology);
 
 int adm_multi_ch_copp_open(int port, int path, int rate, int mode,
diff --git a/include/sound/q6asm-v2.h b/include/sound/q6asm-v2.h
index 182da1c..a436a6e 100644
--- a/include/sound/q6asm-v2.h
+++ b/include/sound/q6asm-v2.h
@@ -140,6 +140,7 @@
 	atomic_t	       cmd_state;
 	/* Relative or absolute TS */
 	uint32_t	       time_flag;
+	atomic_t	       nowait_cmd_cnt;
 	void		       *priv;
 	uint32_t               io_mode;
 	uint64_t	       time_stamp;
diff --git a/mm/filemap.c b/mm/filemap.c
index 79c4b2b..8ed5c5c 100644
--- a/mm/filemap.c
+++ b/mm/filemap.c
@@ -2343,9 +2343,17 @@
 	if (page)
 		goto found;
 
+retry:
 	page = __page_cache_alloc(gfp_mask & ~gfp_notmask);
 	if (!page)
 		return NULL;
+
+	if (is_cma_pageblock(page)) {
+		__free_page(page);
+		gfp_notmask |= __GFP_MOVABLE;
+		goto retry;
+	}
+
 	status = add_to_page_cache_lru(page, mapping, index,
 						GFP_KERNEL & ~gfp_notmask);
 	if (unlikely(status)) {
diff --git a/mm/memory.c b/mm/memory.c
index 174fcaa..c130853 100644
--- a/mm/memory.c
+++ b/mm/memory.c
@@ -49,6 +49,7 @@
 #include <linux/rmap.h>
 #include <linux/export.h>
 #include <linux/delayacct.h>
+#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/writeback.h>
 #include <linux/memcontrol.h>
@@ -2912,6 +2913,16 @@
 	entry = pte_to_swp_entry(orig_pte);
 	if (unlikely(non_swap_entry(entry))) {
 		if (is_migration_entry(entry)) {
+#ifdef CONFIG_DMA_CMA
+			/*
+			 * FIXME: mszyprow: cruel, brute-force method for
+			 * letting cma/migration to finish it's job without
+			 * stealing the lock migration_entry_wait() and creating
+			 * a live-lock on the faulted page
+			 * (page->_count == 2 migration failure issue)
+			 */
+			mdelay(10);
+#endif
 			migration_entry_wait(mm, pmd, address);
 		} else if (is_hwpoison_entry(entry)) {
 			ret = VM_FAULT_HWPOISON;
diff --git a/mm/page_alloc.c b/mm/page_alloc.c
index 92dd060..bd3f0f3 100644
--- a/mm/page_alloc.c
+++ b/mm/page_alloc.c
@@ -758,6 +758,11 @@
 }
 
 #ifdef CONFIG_CMA
+bool is_cma_pageblock(struct page *page)
+{
+	return get_pageblock_migratetype(page) == MIGRATE_CMA;
+}
+
 /* Free whole pageblock and set it's migration type to MIGRATE_CMA. */
 void __init init_cma_reserved_pageblock(struct page *page)
 {
@@ -5422,7 +5427,7 @@
 	pfn &= (PAGES_PER_SECTION-1);
 	return (pfn >> pageblock_order) * NR_PAGEBLOCK_BITS;
 #else
-	pfn = pfn - zone->zone_start_pfn;
+	pfn = pfn - round_down(zone->zone_start_pfn, pageblock_nr_pages);
 	return (pfn >> pageblock_order) * NR_PAGEBLOCK_BITS;
 #endif /* CONFIG_SPARSEMEM */
 }
@@ -5677,7 +5682,7 @@
 	};
 	INIT_LIST_HEAD(&cc.migratepages);
 
-	migrate_prep_local();
+	migrate_prep();
 
 	while (pfn < end || !list_empty(&cc.migratepages)) {
 		if (fatal_signal_pending(current)) {
@@ -5701,7 +5706,7 @@
 
 		ret = migrate_pages(&cc.migratepages,
 				    __alloc_contig_migrate_alloc,
-				    0, false, true);
+				    0, false, MIGRATE_SYNC);
 	}
 
 	putback_lru_pages(&cc.migratepages);
diff --git a/net/bluetooth/mgmt.c b/net/bluetooth/mgmt.c
index 23526f3..8568dae7 100644
--- a/net/bluetooth/mgmt.c
+++ b/net/bluetooth/mgmt.c
@@ -3094,6 +3094,12 @@
 		goto no_auto_confirm;
 	}
 
+	/* Show bonding dialog if neither side requires no bonding */
+	if ((conn->auth_type > 0x01) && (conn->remote_auth > 0x01)) {
+		if (!loc_mitm && !rem_mitm)
+			value = 0;
+		goto no_auto_confirm;
+	}
 
 	if ((!loc_mitm || rem_cap == 0x03) && (!rem_mitm || loc_cap == 0x03))
 		ev.auto_confirm = 1;
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 9669d4a..8cec741 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -397,8 +397,10 @@
 		if (sco_pi(sk)->conn) {
 			sk->sk_state = BT_DISCONN;
 			sco_sock_set_timer(sk, SCO_DISCONN_TIMEOUT);
-			hci_conn_put(sco_pi(sk)->conn->hcon);
-			sco_pi(sk)->conn->hcon = NULL;
+			if (sco_pi(sk)->conn->hcon != NULL) {
+				hci_conn_put(sco_pi(sk)->conn->hcon);
+				sco_pi(sk)->conn->hcon = NULL;
+			}
 		} else
 			sco_chan_del(sk, ECONNRESET);
 		break;
diff --git a/net/ipv4/netfilter/Kconfig b/net/ipv4/netfilter/Kconfig
index a1dce8a..49650e9 100644
--- a/net/ipv4/netfilter/Kconfig
+++ b/net/ipv4/netfilter/Kconfig
@@ -184,6 +184,17 @@
 
 	  To compile it as a module, choose M here.  If unsure, say N.
 
+config IP_NF_TARGET_NATTYPE_MODULE
+	tristate "NATTYPE target support"
+	depends on NF_NAT
+	default m if NETFILTER_ADVANCED=n
+	help
+	  NATTYPE is a special case of NAT: used to support FULL Cone NAT
+	  and ADDRESS Restricted Cone NAT. All incoming connections are
+	  allowed if there is an outgoing connection using that port.
+
+	  To compile it as a module, choose M here.  If unsure, say N.
+
 config IP_NF_TARGET_NETMAP
 	tristate "NETMAP target support"
 	depends on NF_NAT
diff --git a/net/ipv4/netfilter/Makefile b/net/ipv4/netfilter/Makefile
index 2177507..291934b 100644
--- a/net/ipv4/netfilter/Makefile
+++ b/net/ipv4/netfilter/Makefile
@@ -55,6 +55,7 @@
 obj-$(CONFIG_IP_NF_TARGET_CLUSTERIP) += ipt_CLUSTERIP.o
 obj-$(CONFIG_IP_NF_TARGET_ECN) += ipt_ECN.o
 obj-$(CONFIG_IP_NF_TARGET_MASQUERADE) += ipt_MASQUERADE.o
+obj-$(CONFIG_IP_NF_TARGET_NATTYPE_MODULE) += ipt_NATTYPE.o
 obj-$(CONFIG_IP_NF_TARGET_NETMAP) += ipt_NETMAP.o
 obj-$(CONFIG_IP_NF_TARGET_REDIRECT) += ipt_REDIRECT.o
 obj-$(CONFIG_IP_NF_TARGET_REJECT) += ipt_REJECT.o
diff --git a/net/ipv4/netfilter/ipt_NATTYPE.c b/net/ipv4/netfilter/ipt_NATTYPE.c
new file mode 100644
index 0000000..6b28794
--- /dev/null
+++ b/net/ipv4/netfilter/ipt_NATTYPE.c
@@ -0,0 +1,608 @@
+/*
+ * net/ipv4/netfilter/ipt_NATTYPE.c
+ * Endpoint Independent, Address Restricted and Port-Address Restricted
+ * NAT types' kernel side implementation.
+ *
+ * (C) Copyright 2011, Ubicom, Inc.
+ *
+ * This file is part of the Ubicom32 Linux Kernel Port.
+ *
+ * The Ubicom32 Linux Kernel Port is free software: you can redistribute
+ * it and/or modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation, either version 2 of the
+ * License, or (at your option) any later version.
+ *
+ * The Ubicom32 Linux Kernel Port is distributed in the hope that it
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
+ * the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with the Ubicom32 Linux Kernel Port.  If not,
+ * see <http://www.gnu.org/licenses/>.
+ *
+ * Ubicom32 implementation derived from
+ * Cameo's implementation(with many thanks):
+ */
+#include <linux/types.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#include <linux/netfilter.h>
+#include <linux/netfilter_ipv4.h>
+#include <linux/module.h>
+#include <net/protocol.h>
+#include <net/checksum.h>
+#include <net/ip.h>
+#include <linux/tcp.h>
+#include <net/netfilter/nf_conntrack.h>
+#include <net/netfilter/nf_conntrack_core.h>
+#include <net/netfilter/nf_nat_rule.h>
+#include <linux/netfilter/x_tables.h>
+#include <linux/netfilter_ipv4/ipt_NATTYPE.h>
+#include <linux/atomic.h>
+
+#if !defined(NATTYPE_DEBUG)
+#define DEBUGP(type, args...)
+#else
+static const char * const types[] = {"TYPE_PORT_ADDRESS_RESTRICTED",
+			"TYPE_ENDPOINT_INDEPENDENT",
+			"TYPE_ADDRESS_RESTRICTED"};
+static const char * const modes[] = {"MODE_DNAT", "MODE_FORWARD_IN",
+			"MODE_FORWARD_OUT"};
+#define DEBUGP(args...) printk(KERN_DEBUG args);
+#endif
+
+/*
+ * TODO: Add magic value checks to data structure.
+ */
+struct ipt_nattype {
+	struct list_head list;
+	struct timer_list timeout;
+	unsigned short proto;		/* Protocol: TCP or UDP */
+	struct nf_nat_ipv4_range range;	/* LAN side source information */
+	unsigned short nat_port;	/* Routed NAT port */
+	unsigned int dest_addr;	/* Original egress packets destination addr */
+	unsigned short dest_port;/* Original egress packets destination port */
+};
+
+/*
+ * TODO: It might be better to use a hash table for performance in
+ * heavy traffic.
+ */
+static LIST_HEAD(nattype_list);
+static DEFINE_SPINLOCK(nattype_lock);
+
+/*
+ * nattype_nte_debug_print()
+ */
+static void nattype_nte_debug_print(const struct ipt_nattype *nte,
+				const char *s)
+{
+#if defined(NATTYPE_DEBUG)
+	DEBUGP("%p: %s - proto[%d], src[%pI4:%d], nat[<x>:%d], dest[%pI4:%d]\n",
+		nte, s, nte->proto,
+		&nte->range.min_ip, ntohs(nte->range.min.all),
+		ntohs(nte->nat_port),
+		&nte->dest_addr, ntohs(nte->dest_port));
+#endif
+}
+
+/*
+ * nattype_free()
+ *	Free the object.
+ */
+static void nattype_free(struct ipt_nattype *nte)
+{
+	nattype_nte_debug_print(nte, "free");
+	kfree(nte);
+}
+
+/*
+ * nattype_refresh_timer()
+ *	Refresh the timer for this object.
+ */
+static bool nattype_refresh_timer(struct ipt_nattype *nte)
+{
+
+	if (del_timer(&nte->timeout)) {
+		nte->timeout.expires = jiffies + NATTYPE_TIMEOUT * HZ;
+		add_timer(&nte->timeout);
+		return true;
+	}
+	return false;
+}
+
+/*
+ * nattype_timer_timeout()
+ *	The timer has gone off, self-destruct
+ */
+static void nattype_timer_timeout(unsigned long in_nattype)
+{
+	struct ipt_nattype *nte = (void *) in_nattype;
+
+	/*
+	 * The race with list deletion is solved by ensuring
+	 * that either this code or the list deletion code
+	 * but not both will remove the oject.
+	 */
+	nattype_nte_debug_print(nte, "timeout");
+	spin_lock_bh(&nattype_lock);
+	list_del(&nte->list);
+	spin_unlock_bh(&nattype_lock);
+	nattype_free(nte);
+}
+
+/*
+ * nattype_packet_in_match()
+ *	Ingress packet, try to match with this nattype entry.
+ */
+static bool nattype_packet_in_match(const struct ipt_nattype *nte,
+				struct sk_buff *skb,
+				const struct ipt_nattype_info *info)
+{
+	const struct iphdr *iph = ip_hdr(skb);
+	uint16_t dst_port = 0;
+
+	/*
+	 * If the protocols are not the same, no sense in looking
+	 * further.
+	 */
+	if (nte->proto != iph->protocol) {
+		DEBUGP("nattype_packet_in_match: protocol failed: nte proto:" \
+			" %d, packet proto: %d\n",
+			nte->proto, iph->protocol);
+		return false;
+	}
+
+	/*
+	 * In ADDRESS_RESTRICT, the egress destination must match the source
+	 * of this ingress packet.
+	 */
+	if (info->type == TYPE_ADDRESS_RESTRICTED) {
+		if (nte->dest_addr != iph->saddr) {
+			DEBUGP("nattype_packet_in_match: dest/src check" \
+				" failed: dest_addr: %pI4, src dest: %pI4\n",
+				&nte->dest_addr, &iph->saddr);
+			return false;
+		}
+	}
+
+	/*
+	 * Obtain the destination port value for TCP or UDP.  The nattype
+	 * entries are stored in native (not host).
+	 */
+	if (iph->protocol == IPPROTO_TCP) {
+		struct tcphdr _tcph;
+		struct tcphdr *tcph;
+		tcph = skb_header_pointer(skb, ip_hdrlen(skb),
+			sizeof(_tcph), &_tcph);
+		if (!tcph)
+			return false;
+		dst_port = tcph->dest;
+	} else if (iph->protocol == IPPROTO_UDP) {
+		struct udphdr _udph;
+		struct udphdr *udph;
+		udph = skb_header_pointer(skb, ip_hdrlen(skb),
+			sizeof(_udph), &_udph);
+		if (!udph)
+			return false;
+		dst_port = udph->dest;
+	}
+
+	/*
+	 * Our NAT port must match the ingress pacekt's destination packet.
+	 */
+	if (nte->nat_port != dst_port) {
+		DEBUGP("nattype_packet_in_match fail: nat port: %d," \
+			" dest_port: %d\n",
+			ntohs(nte->nat_port), ntohs(dst_port));
+		return false;
+	}
+
+	/*
+	 * In either EI or AR mode, the ingress packet's src port
+	 * can be anything.
+	 */
+	nattype_nte_debug_print(nte, "INGRESS MATCH");
+	return true;
+}
+
+/*
+ * nattype_compare
+ *	Compare two entries, return true if relevant fields are the same.
+ */
+static bool nattype_compare(struct ipt_nattype *n1, struct ipt_nattype *n2)
+{
+	/*
+	 * Protocol compare.
+	 */
+	if (n1->proto != n2->proto) {
+		DEBUGP("nattype_compare: protocol mismatch: %d:%d\n",
+				n1->proto, n2->proto);
+		return false;
+	}
+
+	/*
+	 * LAN Source compare.
+	 * Since we always keep min/max values the same,
+	 * just compare the min values.
+	 */
+	if (n1->range.min_ip != n2->range.min_ip) {
+		DEBUGP("nattype_compare: r.min_ip mismatch: %pI4:%pI4\n",
+				&n1->range.min_ip, &n2->range.min_ip);
+		return false;
+	}
+
+	if (n1->range.min.all != n2->range.min.all) {
+		DEBUGP("nattype_compare: r.min mismatch: %d:%d\n",
+				ntohs(n1->range.min.all),
+				ntohs(n2->range.min.all));
+		return false;
+	}
+
+	/*
+	 * NAT port
+	 */
+	if (n1->nat_port != n2->nat_port) {
+		DEBUGP("nattype_compare: nat_port mistmatch: %d:%d\n",
+				ntohs(n1->nat_port), ntohs(n2->nat_port));
+		return false;
+	}
+
+	/*
+	 * Destination compare
+	 */
+	if (n1->dest_addr != n2->dest_addr) {
+		DEBUGP("nattype_compare: dest_addr mismatch: %pI4:%pI4\n",
+				&n1->dest_addr, &n2->dest_addr);
+		return false;
+	}
+
+	if (n1->dest_port != n2->dest_port) {
+		DEBUGP("nattype_compare: dest_port mismatch: %d:%d\n",
+				ntohs(n1->dest_port), ntohs(n2->dest_port));
+		return false;
+	}
+	return true;
+}
+
+/*
+ * nattype_nat()
+ *	Ingress packet on PRE_ROUTING hook, find match, update conntrack
+ *      to allow
+ */
+static unsigned int nattype_nat(struct sk_buff *skb,
+				const struct xt_action_param *par)
+{
+	struct ipt_nattype *nte;
+
+	if (par->hooknum != NF_INET_PRE_ROUTING)
+		return XT_CONTINUE;
+	spin_lock_bh(&nattype_lock);
+	list_for_each_entry(nte, &nattype_list, list) {
+		struct nf_conn *ct;
+		enum ip_conntrack_info ctinfo;
+		struct nf_nat_ipv4_range newrange;
+		unsigned int ret;
+
+		if (!nattype_packet_in_match(nte, skb, par->targinfo))
+			continue;
+
+		/*
+		 * Copy the LAN source data into the ingress' pacekts
+		 * conntrack in the reply direction.
+		 */
+		newrange = nte->range;
+		spin_unlock_bh(&nattype_lock);
+
+		/*
+		 * Find the ingress packet's conntrack.
+		 */
+		ct = nf_ct_get(skb, &ctinfo);
+		if (!ct) {
+			DEBUGP("ingress packet conntrack not found\n");
+			return XT_CONTINUE;
+		}
+
+		/*
+		 * Expand the ingress conntrack to include the reply as source
+		 */
+		DEBUGP("Expand ingress conntrack=%p, type=%d, src[%pI4:%d]\n",
+			ct, ctinfo, &newrange.min_ip, ntohs(newrange.min.all));
+		ret = nf_nat_setup_info(ct, &newrange, NF_NAT_MANIP_DST);
+		DEBUGP("Expand returned: %d\n", ret);
+		return ret;
+	}
+	spin_unlock_bh(&nattype_lock);
+	return XT_CONTINUE;
+}
+
+/*
+ * nattype_forward()
+ *	Ingress and Egress packet forwarding hook
+ */
+static unsigned int nattype_forward(struct sk_buff *skb,
+				const struct xt_action_param *par)
+{
+	const struct iphdr *iph = ip_hdr(skb);
+	void *protoh = (void *)iph + iph->ihl * 4;
+	struct ipt_nattype *nte;
+	struct ipt_nattype *nte2;
+	struct nf_conn *ct;
+	enum ip_conntrack_info ctinfo;
+	const struct ipt_nattype_info *info = par->targinfo;
+	uint16_t nat_port;
+
+	if (par->hooknum != NF_INET_FORWARD)
+		return XT_CONTINUE;
+
+	/*
+	 * Ingress packet, refresh the timer if we find an entry.
+	 */
+	if (info->mode == MODE_FORWARD_IN) {
+		spin_lock_bh(&nattype_lock);
+		list_for_each_entry(nte, &nattype_list, list) {
+			/*
+			 * Compare the ingress packet with the existing
+			 * entries looking for a match.
+			 */
+			if (!nattype_packet_in_match(nte, skb, info))
+				continue;
+
+			/*
+			 * Refresh the timer, if we fail, break
+			 * out and forward fail as though we never
+			 * found the entry.
+			 */
+			if (!nattype_refresh_timer(nte))
+				break;
+
+			/*
+			 * The entry is found and refreshed, the
+			 * entry values should not change so print
+			 * them outside the lock.
+			 */
+			spin_unlock_bh(&nattype_lock);
+			nattype_nte_debug_print(nte, "refresh");
+			DEBUGP("FORWARD_IN_ACCEPT\n");
+			return NF_ACCEPT;
+		}
+		spin_unlock_bh(&nattype_lock);
+		DEBUGP("FORWARD_IN_FAIL\n");
+		return XT_CONTINUE;
+	}
+
+	/*
+	 * Egress packet, create a new rule in our list.  If conntrack does
+	 * not have an entry, skip this packet.
+	 */
+	ct = nf_ct_get(skb, &ctinfo);
+	if (!ct || (ctinfo == IP_CT_NEW && ctinfo == IP_CT_RELATED))
+		return XT_CONTINUE;
+
+	nat_port = ct->tuplehash[IP_CT_DIR_REPLY].tuple.dst.u.all;
+
+	/*
+	 * Allocate a new entry
+	 */
+	nte = kzalloc(sizeof(struct ipt_nattype), GFP_ATOMIC | __GFP_NOWARN);
+	if (!nte) {
+		DEBUGP("kernel malloc fail\n");
+		return XT_CONTINUE;
+	}
+
+	INIT_LIST_HEAD(&nte->list);
+
+	nte->proto = iph->protocol;
+	nte->nat_port = nat_port;
+	nte->dest_addr = iph->daddr;
+	nte->range.max_ip = nte->range.min_ip = iph->saddr;
+
+	/*
+	 * TOOD: Would it be better to get this information from the
+	 * conntrack instead of the headers.
+	 */
+	if (iph->protocol == IPPROTO_TCP) {
+		nte->range.max.tcp.port = nte->range.min.tcp.port =
+					((struct tcphdr *)protoh)->source;
+		nte->dest_port = ((struct tcphdr *)protoh)->dest;
+	} else if (iph->protocol == IPPROTO_UDP) {
+		nte->range.max.udp.port = nte->range.min.udp.port =
+					((struct udphdr *)protoh)->source;
+		nte->dest_port = ((struct udphdr *)protoh)->dest;
+	}
+	nte->range.flags = (NF_NAT_RANGE_MAP_IPS |
+			NF_NAT_RANGE_PROTO_SPECIFIED);
+
+	/*
+	 * Initilize the self-destruct timer.
+	 */
+	init_timer(&nte->timeout);
+	nte->timeout.data = (unsigned long)nte;
+	nte->timeout.function = nattype_timer_timeout;
+
+	/*
+	 * We have created the new nte; however, it might not be unique.
+	 * Search the list for a matching entry.  If found, throw away
+	 * the new entry and refresh the old.  If not found, atomically
+	 * insert the new entry on the list.
+	 */
+	spin_lock_bh(&nattype_lock);
+	list_for_each_entry(nte2, &nattype_list, list) {
+		if (!nattype_compare(nte, nte2))
+			continue;
+
+		/*
+		 * If we can not refresh this entry, insert our new
+		 * entry as this one is timed out and will be removed
+		 * from the list shortly.
+		 */
+		if (!nattype_refresh_timer(nte2))
+			break;
+
+		/*
+		 * Found and refreshed an existing entry.  Its values
+		 * do not change so print the values outside of the lock.
+		 *
+		 * Free up the new entry.
+		 */
+		spin_unlock_bh(&nattype_lock);
+		nattype_nte_debug_print(nte2, "refresh");
+		nattype_free(nte);
+		return XT_CONTINUE;
+	}
+
+	/*
+	 * Add the new entry to the list.
+	 */
+	nte->timeout.expires = jiffies + (NATTYPE_TIMEOUT  * HZ);
+	add_timer(&nte->timeout);
+	list_add(&nte->list, &nattype_list);
+	spin_unlock_bh(&nattype_lock);
+	nattype_nte_debug_print(nte, "ADD");
+	return XT_CONTINUE;
+}
+
+/*
+ * nattype_target()
+ *	One of the iptables hooks has a packet for us to analyze, do so.
+ */
+static unsigned int nattype_target(struct sk_buff *skb,
+				const struct xt_action_param *par)
+{
+	const struct ipt_nattype_info *info = par->targinfo;
+	const struct iphdr *iph = ip_hdr(skb);
+
+	/*
+	 * The default behavior for Linux is PORT and ADDRESS restricted. So
+	 * we do not need to create rules/entries if we are in that mode.
+	 */
+	if (info->type == TYPE_PORT_ADDRESS_RESTRICTED)
+		return XT_CONTINUE;
+
+	/*
+	 * Check if we have enough data in the skb.
+	 */
+	if (skb->len < ip_hdrlen(skb))
+		return XT_CONTINUE;
+
+	/*
+	 * We can not perform endpoint filtering on anything but UDP and TCP.
+	 */
+	if ((iph->protocol != IPPROTO_TCP) && (iph->protocol != IPPROTO_UDP))
+		return XT_CONTINUE;
+
+	/*
+	 * Check for LAND attack and ignore.
+	 */
+	if (iph->daddr == iph->saddr)
+		return XT_CONTINUE;
+
+	/*
+	 * Check that we have valid source and destination addresses.
+	 */
+	if ((iph->daddr == (__be32)0) || (iph->saddr == (__be32)0))
+		return XT_CONTINUE;
+
+	DEBUGP("nattype_target: type = %s, mode = %s\n",
+		types[info->type], modes[info->mode]);
+
+	/*
+	 * TODO: why have mode at all since par->hooknum provides
+	 * this information?
+	 */
+	switch (info->mode) {
+	case MODE_DNAT:
+		return nattype_nat(skb, par);
+	case MODE_FORWARD_OUT:
+	case MODE_FORWARD_IN:
+		return nattype_forward(skb, par);
+	}
+	return XT_CONTINUE;
+}
+
+/*
+ * nattype_check()
+ *	check info (mode/type) set by iptables.
+ */
+static int nattype_check(const struct xt_tgchk_param *par)
+{
+	const struct ipt_nattype_info *info = par->targinfo;
+	struct list_head *cur, *tmp;
+
+	if ((info->type != TYPE_PORT_ADDRESS_RESTRICTED) &&
+		(info->type != TYPE_ENDPOINT_INDEPENDENT) &&
+		(info->type != TYPE_ADDRESS_RESTRICTED)) {
+		DEBUGP("nattype_check: unknown type: %d\n", info->type);
+		return -EINVAL;
+	}
+
+	if (info->mode != MODE_DNAT && info->mode != MODE_FORWARD_IN &&
+		info->mode != MODE_FORWARD_OUT) {
+		DEBUGP("nattype_check: unknown mode - %d.\n", info->mode);
+		return -EINVAL;
+	}
+
+	DEBUGP("nattype_check: type = %s, mode = %s\n",
+		types[info->type], modes[info->mode]);
+
+	if (par->hook_mask & ~((1 << NF_INET_PRE_ROUTING) |
+		(1 << NF_INET_FORWARD))) {
+		DEBUGP("nattype_check: bad hooks %x.\n", par->hook_mask);
+		return -EINVAL;
+	}
+
+	/*
+	 * Remove all entries from the nattype list.
+	 */
+drain:
+	spin_lock_bh(&nattype_lock);
+	list_for_each_safe(cur, tmp, &nattype_list) {
+		struct ipt_nattype *nte = (void *)cur;
+
+		/*
+		 * If the timeout is in process, it will tear
+		 * us down.  Since it is waiting on the spinlock
+		 * we have to give up the spinlock to give the
+		 * timeout on another CPU a chance to run.
+		 */
+		if (!del_timer(&nte->timeout)) {
+			spin_unlock_bh(&nattype_lock);
+			goto drain;
+		}
+
+		DEBUGP("%p: removing from list\n", nte);
+		list_del(&nte->list);
+		spin_unlock_bh(&nattype_lock);
+		nattype_free(nte);
+		goto drain;
+	}
+	spin_unlock_bh(&nattype_lock);
+	return 0;
+}
+
+static struct xt_target nattype = {
+	.name		= "NATTYPE",
+	.family		= NFPROTO_IPV4,
+	.target		= nattype_target,
+	.checkentry	= nattype_check,
+	.targetsize	= sizeof(struct ipt_nattype_info),
+	.hooks		= ((1 << NF_INET_PRE_ROUTING) |
+				(1 << NF_INET_FORWARD)),
+	.me		= THIS_MODULE,
+};
+
+static int __init init(void)
+{
+	return xt_register_target(&nattype);
+}
+
+static void __exit fini(void)
+{
+	xt_unregister_target(&nattype);
+}
+
+module_init(init);
+module_exit(fini);
+
+MODULE_LICENSE("GPL");
diff --git a/sound/soc/codecs/Kconfig b/sound/soc/codecs/Kconfig
index b211ac4..1b46499 100644
--- a/sound/soc/codecs/Kconfig
+++ b/sound/soc/codecs/Kconfig
@@ -448,3 +448,6 @@
 
 config SND_SOC_MSM_STUB
 	tristate
+
+config SND_SOC_MSM_HDMI_CODEC_RX
+	tristate
diff --git a/sound/soc/codecs/Makefile b/sound/soc/codecs/Makefile
index 1c9d86b..42121fe 100644
--- a/sound/soc/codecs/Makefile
+++ b/sound/soc/codecs/Makefile
@@ -102,6 +102,8 @@
 
 snd-soc-timpani-objs := timpani.o
 snd-soc-msm-stub-objs := msm_stub.o
+snd-soc-msm-hdmi-rx-objs := msm_hdmi_codec_rx.o
+
 # Amp
 snd-soc-max9877-objs := max9877.o
 snd-soc-tpa6130a2-objs := tpa6130a2.o
@@ -208,6 +210,7 @@
 obj-$(CONFIG_SND_SOC_WM9713)	+= snd-soc-wm9713.o
 obj-$(CONFIG_SND_SOC_WM_HUBS)	+= snd-soc-wm-hubs.o
 obj-$(CONFIG_SND_SOC_MSM_STUB)  += snd-soc-msm-stub.o
+obj-$(CONFIG_SND_SOC_MSM_HDMI_CODEC_RX)  += snd-soc-msm-hdmi-rx.o
 
 # Amp
 obj-$(CONFIG_SND_SOC_MAX9877)	+= snd-soc-max9877.o
diff --git a/sound/soc/codecs/msm_hdmi_codec_rx.c b/sound/soc/codecs/msm_hdmi_codec_rx.c
new file mode 100644
index 0000000..0000a1a
--- /dev/null
+++ b/sound/soc/codecs/msm_hdmi_codec_rx.c
@@ -0,0 +1,207 @@
+/* Copyright (c) 2012, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <sound/core.h>
+#include <sound/pcm.h>
+#include <sound/soc.h>
+#include <mach/msm_hdmi_audio_codec.h>
+
+#define MSM_HDMI_PCM_RATES	SNDRV_PCM_RATE_48000
+
+struct msm_hdmi_audio_codec_rx_data {
+	struct platform_device *hdmi_core_pdev;
+	struct msm_hdmi_audio_codec_ops hdmi_ops;
+};
+
+static int msm_hdmi_audio_codec_rx_dai_hw_params(
+		struct snd_pcm_substream *substream,
+		struct snd_pcm_hw_params *params,
+		struct snd_soc_dai *dai)
+{
+	u32 channel_allocation = 0;
+	u32 level_shift  = 0; /* 0dB */
+	bool down_mix = 0;
+	u32 num_channels = params_channels(params);
+
+	struct msm_hdmi_audio_codec_rx_data *codec_data =
+			dev_get_drvdata(dai->codec->dev);
+
+	switch (num_channels) {
+	case 2:
+		channel_allocation  = 0;
+		break;
+	case 6:
+		channel_allocation  = 0x0B;
+		break;
+	case 8:
+		channel_allocation  = 0x13;
+		break;
+	default:
+		dev_err(dai->dev, "invalid Channels = %u\n", num_channels);
+		return -EINVAL;
+	}
+
+	dev_dbg(dai->dev, "%s() num_ch %u  samplerate %u channel_allocation = %u\n",
+		__func__, num_channels, params_rate(params),
+		channel_allocation);
+
+	codec_data->hdmi_ops.audio_info_setup(codec_data->hdmi_core_pdev,
+			num_channels, channel_allocation,
+			level_shift, down_mix);
+
+	return 0;
+}
+
+static struct snd_soc_dai_ops msm_hdmi_audio_codec_rx_dai_ops = {
+	.hw_params	= msm_hdmi_audio_codec_rx_dai_hw_params,
+};
+
+static int msm_hdmi_audio_codec_rx_probe(struct snd_soc_codec *codec)
+{
+	struct msm_hdmi_audio_codec_rx_data *codec_data;
+	struct device_node *of_node_parent = NULL;
+
+	codec_data = kzalloc(sizeof(struct msm_hdmi_audio_codec_rx_data),
+		GFP_KERNEL);
+
+	if (!codec_data) {
+		dev_err(codec->dev, "%s(): fail to allocate dai data\n",
+				__func__);
+		return -ENOMEM;
+	}
+
+	of_node_parent = of_get_parent(codec->dev->of_node);
+	if (!of_node_parent) {
+		dev_err(codec->dev, "%s(): Parent device tree node not found\n",
+				__func__);
+		kfree(codec_data);
+		return -ENODEV;
+	}
+
+	codec_data->hdmi_core_pdev = of_find_device_by_node(of_node_parent);
+	if (!codec_data->hdmi_core_pdev) {
+		dev_err(codec->dev, "%s(): can't get parent pdev\n", __func__);
+		kfree(codec_data);
+		return -ENODEV;
+	}
+
+	if (msm_hdmi_register_audio_codec(codec_data->hdmi_core_pdev,
+				&codec_data->hdmi_ops)) {
+		dev_err(codec->dev, "%s(): can't register with hdmi core",
+				__func__);
+		kfree(codec_data);
+		return -ENODEV;
+	}
+
+	dev_set_drvdata(codec->dev, codec_data);
+
+	dev_dbg(codec->dev, "%s(): registerd %s with HDMI core\n",
+		__func__, codec->name);
+
+	return 0;
+}
+
+static int msm_hdmi_audio_codec_rx_remove(struct snd_soc_codec *codec)
+{
+	struct msm_hdmi_audio_codec_rx_data *codec_data;
+
+	codec_data = dev_get_drvdata(codec->dev);
+	kfree(codec_data);
+
+	return 0;
+}
+
+static struct snd_soc_dai_driver msm_hdmi_audio_codec_rx_dais[] = {
+	{
+		.name = "msm_hdmi_audio_codec_rx_dai",
+		.playback = {
+			.stream_name = "HDMI Playback",
+			.channels_min = 1,
+			.channels_max = 8,
+			.rate_min = 48000,
+			.rate_max = 48000,
+			.rates = MSM_HDMI_PCM_RATES,
+			.formats = SNDRV_PCM_FMTBIT_S16_LE,
+		},
+		.ops = &msm_hdmi_audio_codec_rx_dai_ops,
+	},
+};
+
+static struct snd_soc_codec_driver msm_hdmi_audio_codec_rx_soc_driver = {
+	.probe = msm_hdmi_audio_codec_rx_probe,
+	.remove =  msm_hdmi_audio_codec_rx_remove,
+};
+
+static int __devinit msm_hdmi_audio_codec_rx_plat_probe(
+		struct platform_device *pdev)
+{
+	dev_dbg(&pdev->dev, "%s(): orginal dev name  = %s, id = %d\n",
+		__func__, dev_name(&pdev->dev), pdev->id);
+
+	if (pdev->dev.of_node) {
+		dev_dbg(&pdev->dev, "%s(): node full name = %s,  name = %s\n",
+			__func__, pdev->dev.of_node->full_name,
+			pdev->dev.of_node->name);
+		dev_set_name(&pdev->dev, "%s", "msm-hdmi-audio-codec-rx");
+	} else
+		dev_err(&pdev->dev, "%s(): platfrom data not from device tree\n",
+				__func__);
+
+	dev_dbg(&pdev->dev, "%s(): new dev name %s\n", __func__,
+		dev_name(&pdev->dev));
+
+	return snd_soc_register_codec(&pdev->dev,
+		&msm_hdmi_audio_codec_rx_soc_driver,
+		msm_hdmi_audio_codec_rx_dais,
+		ARRAY_SIZE(msm_hdmi_audio_codec_rx_dais));
+}
+
+static int __devexit msm_hdmi_audio_codec_rx_plat_remove(
+		struct platform_device *pdev)
+{
+	snd_soc_unregister_codec(&pdev->dev);
+	return 0;
+}
+static const struct of_device_id msm_hdmi_audio_codec_rx_dt_match[] = {
+	{ .compatible = "qcom,msm-hdmi-audio-codec-rx", },
+	{}
+};
+MODULE_DEVICE_TABLE(of, msm_hdmi_codec_dt_match);
+
+static struct platform_driver msm_hdmi_audio_codec_rx_driver = {
+	.driver = {
+		.name = "msm-hdmi-audio-codec-rx",
+		.owner = THIS_MODULE,
+		.of_match_table = msm_hdmi_audio_codec_rx_dt_match,
+	},
+	.probe = msm_hdmi_audio_codec_rx_plat_probe,
+	.remove = __devexit_p(msm_hdmi_audio_codec_rx_plat_remove),
+};
+
+static int __init msm_hdmi_audio_codec_rx_init(void)
+{
+	return platform_driver_register(&msm_hdmi_audio_codec_rx_driver);
+}
+module_init(msm_hdmi_audio_codec_rx_init);
+
+static void __exit msm_hdmi_audio_codec_rx_exit(void)
+{
+	platform_driver_unregister(&msm_hdmi_audio_codec_rx_driver);
+}
+module_exit(msm_hdmi_audio_codec_rx_exit);
+
+MODULE_DESCRIPTION("MSM HDMI CODEC driver");
+MODULE_VERSION("1.0");
+MODULE_LICENSE("GPL v2");
diff --git a/sound/soc/codecs/msm_stub.c b/sound/soc/codecs/msm_stub.c
index 7e603b4..0143e51 100644
--- a/sound/soc/codecs/msm_stub.c
+++ b/sound/soc/codecs/msm_stub.c
@@ -34,7 +34,7 @@
 		.capture = { /* Support maximum range */
 			.stream_name = "Record",
 			.channels_min = 1,
-			.channels_max = 4,
+			.channels_max = 8,
 			.rates = SNDRV_PCM_RATE_8000_48000,
 			.formats = SNDRV_PCM_FMTBIT_S16_LE,
 		},
diff --git a/sound/soc/codecs/wcd9320.c b/sound/soc/codecs/wcd9320.c
index ebb0421..76623b1 100644
--- a/sound/soc/codecs/wcd9320.c
+++ b/sound/soc/codecs/wcd9320.c
@@ -54,7 +54,8 @@
 #define TAIKO_SLIM_IRQ_OVERFLOW (1 << 0)
 #define TAIKO_SLIM_IRQ_UNDERFLOW (1 << 1)
 #define TAIKO_SLIM_IRQ_PORT_CLOSED (1 << 2)
-
+#define TAIKO_MCLK_CLK_12P288MHZ 12288000
+#define TAIKO_MCLK_CLK_9P6HZ 9600000
 enum {
 	AIF1_PB = 0,
 	AIF1_CAP,
@@ -3120,11 +3121,7 @@
 static int taiko_set_dai_sysclk(struct snd_soc_dai *dai,
 		int clk_id, unsigned int freq, int dir)
 {
-	struct snd_soc_codec *codec = dai->codec;
-	if (freq == TAIKO_MCLK_CLK_12P288MHZ)
-		snd_soc_write(codec, TAIKO_A_CHIP_CTL, 0x04);
-	else if (freq == TAIKO_MCLK_CLK_9P6HZ)
-		snd_soc_write(codec, TAIKO_A_CHIP_CTL, 0x0A);
+	pr_debug("%s\n", __func__);
 	return 0;
 }
 
@@ -4637,6 +4634,8 @@
 	 */
 	{TAIKO_A_RX_HPH_OCP_CTL, 0xE1, 0x61},
 	{TAIKO_A_RX_COM_OCP_COUNT, 0xFF, 0xFF},
+	{TAIKO_A_RX_HPH_L_TEST, 0x01, 0x01},
+	{TAIKO_A_RX_HPH_R_TEST, 0x01, 0x01},
 
 	/* Initialize gain registers to use register gain */
 	{TAIKO_A_RX_HPH_L_GAIN, 0x20, 0x20},
@@ -4808,6 +4807,11 @@
 	taiko->aux_l_gain = 0x1F;
 	taiko->aux_r_gain = 0x1F;
 	taiko_update_reg_defaults(codec);
+	pr_debug("%s: MCLK Rate = %x\n", __func__, wcd9xxx->mclk_rate);
+	if (wcd9xxx->mclk_rate == TAIKO_MCLK_CLK_12P288MHZ)
+		snd_soc_write(codec, TAIKO_A_CHIP_CTL, 0x04);
+	else if (wcd9xxx->mclk_rate == TAIKO_MCLK_CLK_9P6HZ)
+		snd_soc_write(codec, TAIKO_A_CHIP_CTL, 0x0A);
 	taiko_codec_init_reg(codec);
 	ret = taiko_handle_pdata(taiko);
 	if (IS_ERR_VALUE(ret)) {
diff --git a/sound/soc/msm/Kconfig b/sound/soc/msm/Kconfig
index d5cada7..c957526 100644
--- a/sound/soc/msm/Kconfig
+++ b/sound/soc/msm/Kconfig
@@ -107,7 +107,7 @@
 
 config SND_SOC_MSM_QDSP6V2_INTF
 	bool "SoC Q6 audio driver for MSM8974"
-	depends on MSM_QDSP6_APR
+	depends on MSM_QDSP6_APRV2
 	help
 	 To add support for SoC audio on MSM8974.
 	 This will enable all the platform specific
@@ -170,6 +170,7 @@
 	select SND_SOC_MSM_STUB
 	select SND_SOC_MSM_HOSTLESS_PCM
 	select SND_SOC_WCD9320
+	select SND_SOC_MSM_HDMI_CODEC_RX
 	select SND_DYNAMIC_MINORS
 	select AUDIO_OCMEM
 	help
diff --git a/sound/soc/msm/apq8064.c b/sound/soc/msm/apq8064.c
index 4fe002b..a351f7b 100644
--- a/sound/soc/msm/apq8064.c
+++ b/sound/soc/msm/apq8064.c
@@ -92,7 +92,7 @@
 
 static int msm_btsco_rate = BTSCO_RATE_8KHZ;
 static int msm_btsco_ch = 1;
-
+static int hdmi_rate_variable;
 static int rec_mode = INCALL_REC_MONO;
 
 static struct clk *codec_clk;
@@ -642,11 +642,13 @@
 static const char *spk_function[] = {"Off", "On"};
 static const char *slim0_rx_ch_text[] = {"One", "Two"};
 static const char *slim0_tx_ch_text[] = {"One", "Two", "Three", "Four"};
+static const char * const hdmi_rate[] = {"Default", "Variable"};
 
 static const struct soc_enum msm_enum[] = {
 	SOC_ENUM_SINGLE_EXT(2, spk_function),
 	SOC_ENUM_SINGLE_EXT(2, slim0_rx_ch_text),
 	SOC_ENUM_SINGLE_EXT(4, slim0_tx_ch_text),
+	SOC_ENUM_SINGLE_EXT(2, hdmi_rate),
 };
 
 static const char *btsco_rate_text[] = {"8000", "16000"};
@@ -756,6 +758,21 @@
 	return 0;
 }
 
+static int msm_hdmi_rate_put(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	hdmi_rate_variable = ucontrol->value.integer.value[0];
+	pr_debug("%s: hdmi_rate_variable = %d\n", __func__, hdmi_rate_variable);
+	return 0;
+}
+
+static int msm_hdmi_rate_get(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.integer.value[0] = hdmi_rate_variable;
+	return 0;
+}
+
 static const struct snd_kcontrol_new tabla_msm_controls[] = {
 	SOC_ENUM_EXT("Speaker Function", msm_enum[0], msm_get_spk,
 		msm_set_spk),
@@ -769,6 +786,9 @@
 			msm_incall_rec_mode_get, msm_incall_rec_mode_put),
 	SOC_ENUM_EXT("SLIM_3_RX Channels", msm_enum[1],
 		msm_slim_3_rx_ch_get, msm_slim_3_rx_ch_put),
+	SOC_ENUM_EXT("HDMI RX Rate", msm_enum[3],
+					msm_hdmi_rate_get,
+					msm_hdmi_rate_put),
 };
 
 static void *def_tabla_mbhc_cal(void)
@@ -1328,7 +1348,8 @@
 
 	if (channels->max < 2)
 		channels->min = channels->max = 2;
-	rate->min = rate->max = 48000;
+	if (!hdmi_rate_variable)
+		rate->min = rate->max = 48000;
 
 	return 0;
 }
diff --git a/sound/soc/msm/mdm9615.c b/sound/soc/msm/mdm9615.c
index 7190ae9..433786d 100644
--- a/sound/soc/msm/mdm9615.c
+++ b/sound/soc/msm/mdm9615.c
@@ -316,8 +316,6 @@
 static struct clk *codec_clk;
 static int clk_users;
 
-static int mdm9615_headset_gpios_configured;
-
 static struct snd_soc_jack hs_jack;
 static struct snd_soc_jack button_jack;
 
@@ -2294,57 +2292,6 @@
 	},
 };
 
-static int mdm9615_configure_headset_mic_gpios(void)
-{
-	int ret;
-	struct pm_gpio param = {
-		.direction      = PM_GPIO_DIR_OUT,
-		.output_buffer  = PM_GPIO_OUT_BUF_CMOS,
-		.output_value   = 1,
-		.pull	   = PM_GPIO_PULL_NO,
-		.vin_sel	= PM_GPIO_VIN_S4,
-		.out_strength   = PM_GPIO_STRENGTH_MED,
-		.function       = PM_GPIO_FUNC_NORMAL,
-	};
-
-	ret = gpio_request(PM8018_GPIO_PM_TO_SYS(23), "AV_SWITCH");
-	if (ret) {
-		pr_err("%s: Failed to request gpio %d\n", __func__,
-			PM8018_GPIO_PM_TO_SYS(23));
-		return ret;
-	}
-
-	ret = pm8xxx_gpio_config(PM8018_GPIO_PM_TO_SYS(23), &param);
-	if (ret)
-		pr_err("%s: Failed to configure gpio %d\n", __func__,
-			PM8018_GPIO_PM_TO_SYS(23));
-	else
-		gpio_direction_output(PM8018_GPIO_PM_TO_SYS(23), 0);
-
-	ret = gpio_request(PM8018_GPIO_PM_TO_SYS(35), "US_EURO_SWITCH");
-	if (ret) {
-		pr_err("%s: Failed to request gpio %d\n", __func__,
-			PM8018_GPIO_PM_TO_SYS(35));
-		gpio_free(PM8018_GPIO_PM_TO_SYS(23));
-		return ret;
-	}
-	ret = pm8xxx_gpio_config(PM8018_GPIO_PM_TO_SYS(35), &param);
-	if (ret)
-		pr_err("%s: Failed to configure gpio %d\n", __func__,
-			PM8018_GPIO_PM_TO_SYS(35));
-	else
-		gpio_direction_output(PM8018_GPIO_PM_TO_SYS(35), 0);
-
-	return 0;
-}
-static void mdm9615_free_headset_mic_gpios(void)
-{
-	if (mdm9615_headset_gpios_configured) {
-		gpio_free(PM8018_GPIO_PM_TO_SYS(23));
-		gpio_free(PM8018_GPIO_PM_TO_SYS(35));
-	}
-}
-
 static int __init mdm9615_audio_init(void)
 {
 	int ret;
@@ -2411,12 +2358,6 @@
 		return ret;
 	}
 
-	if (mdm9615_configure_headset_mic_gpios()) {
-		pr_err("%s Fail to configure headset mic gpios\n", __func__);
-		mdm9615_headset_gpios_configured = 0;
-	} else
-		mdm9615_headset_gpios_configured = 1;
-
 	/*
 	 * Irrespective of audio interface type get virtual address
 	 * of LPAIF registers as it may not  be guaranted that I2S
@@ -2444,7 +2385,6 @@
 		pr_err("%s: Not the right machine type\n", __func__);
 		return ;
 	}
-	mdm9615_free_headset_mic_gpios();
 	platform_device_unregister(mdm9615_snd_device_slim);
 	platform_device_unregister(mdm9615_snd_device_i2s);
 	kfree(mbhc_cfg.calibration);
diff --git a/sound/soc/msm/mdm9625.c b/sound/soc/msm/mdm9625.c
index b1822f6..4c7b69d 100644
--- a/sound/soc/msm/mdm9625.c
+++ b/sound/soc/msm/mdm9625.c
@@ -68,7 +68,6 @@
 	unsigned gpio_no;
 	char *gpio_name;
 };
-static bool cdc_mclk_init;
 static struct mutex cdc_mclk_mutex;
 static int mdm9625_mi2s_rx_ch = 1;
 static int mdm9625_mi2s_tx_ch = 1;
@@ -292,25 +291,6 @@
 	return ret;
 }
 
-static int set_codec_mclk(struct snd_soc_pcm_runtime *rtd)
-{
-	int ret = 0;
-	struct snd_soc_dai *codec_dai = rtd->codec_dai;
-	struct snd_soc_card *card = rtd->card;
-	struct mdm9625_machine_data *pdata = snd_soc_card_get_drvdata(card);
-
-	if (cdc_mclk_init == true)
-		return 0;
-	ret = snd_soc_dai_set_sysclk(codec_dai, TAIKO_MCLK_ID, pdata->mclk_freq,
-				     SND_SOC_CLOCK_IN);
-	if (ret < 0) {
-		pr_err("%s: Set codec sys clk failed %x", __func__, ret);
-		return ret;
-	}
-	cdc_mclk_init = true;
-	return 0;
-}
-
 static int mdm9625_mi2s_rx_be_hw_params_fixup(struct snd_soc_pcm_runtime *rtd,
 					     struct snd_pcm_hw_params *params)
 {
@@ -320,7 +300,6 @@
 					SNDRV_PCM_HW_PARAM_CHANNELS);
 	rate->min = rate->max = 48000;
 	channels->min = channels->max = mdm9625_mi2s_rx_ch;
-	set_codec_mclk(rtd);
 	return 0;
 }
 
@@ -333,7 +312,6 @@
 						SNDRV_PCM_HW_PARAM_CHANNELS);
 	rate->min = rate->max = 48000;
 	channels->min = channels->max = mdm9625_mi2s_tx_ch;
-	set_codec_mclk(rtd);
 	return 0;
 }
 
@@ -713,7 +691,6 @@
 
 	mutex_init(&cdc_mclk_mutex);
 	gpio_enable = false;
-	cdc_mclk_init = false;
 	if (!pdev->dev.of_node) {
 		dev_err(&pdev->dev, "No platform supplied from device tree\n");
 		return -EINVAL;
diff --git a/sound/soc/msm/mpq8064.c b/sound/soc/msm/mpq8064.c
index d0bfb76..cef8659 100644
--- a/sound/soc/msm/mpq8064.c
+++ b/sound/soc/msm/mpq8064.c
@@ -30,10 +30,56 @@
 #include "../codecs/wcd9310.h"
 
 /* 8064 machine driver */
-
+#define PM8921_MPP_BASE			(PM8921_GPIO_BASE + PM8921_NR_GPIOS)
+#define PM8821_NR_MPPS		(4)
+#define PM8821_MPP_BASE			(PM8921_MPP_BASE + PM8921_NR_MPPS)
 #define PM8921_GPIO_BASE		NR_GPIO_IRQS
 #define PM8921_GPIO_PM_TO_SYS(pm_gpio)  (pm_gpio - 1 + PM8921_GPIO_BASE)
 
+#define GPIO_EXPANDER_IRQ_BASE	(TABLA_INTERRUPT_BASE + \
+					NR_TABLA_IRQS)
+#define GPIO_EXPANDER_GPIO_BASE	(PM8821_MPP_BASE + PM8821_NR_MPPS)
+
+#define GPIO_EPM_EXPANDER_BASE	GPIO_EXPANDER_GPIO_BASE
+#define SX150X_EPM_NR_GPIOS	16
+#define SX150X_EPM_NR_IRQS	8
+
+#define SX150X_EXP1_GPIO_BASE	(GPIO_EPM_EXPANDER_BASE + \
+					SX150X_EPM_NR_GPIOS)
+#define SX150X_EXP1_IRQ_BASE	(GPIO_EXPANDER_IRQ_BASE + \
+				SX150X_EPM_NR_IRQS)
+#define SX150X_EXP1_NR_IRQS	16
+#define SX150X_EXP1_NR_GPIOS	16
+
+#define SX150X_EXP2_GPIO_BASE	(SX150X_EXP1_GPIO_BASE + \
+					SX150X_EXP1_NR_GPIOS)
+#define SX150X_EXP2_IRQ_BASE	(SX150X_EXP1_IRQ_BASE + SX150X_EXP1_NR_IRQS)
+#define SX150X_EXP2_NR_IRQS	8
+#define SX150X_EXP2_NR_GPIOS	8
+
+#define SX150X_EXP3_GPIO_BASE	(SX150X_EXP2_GPIO_BASE + \
+					SX150X_EXP2_NR_GPIOS)
+#define SX150X_EXP3_IRQ_BASE	(SX150X_EXP2_IRQ_BASE + SX150X_EXP2_NR_IRQS)
+#define SX150X_EXP3_NR_IRQS	8
+#define SX150X_EXP3_NR_GPIOS	8
+
+#define SX150X_EXP4_GPIO_BASE	(SX150X_EXP3_GPIO_BASE + \
+					SX150X_EXP3_NR_GPIOS)
+#define SX150X_EXP4_IRQ_BASE	(SX150X_EXP3_IRQ_BASE + SX150X_EXP3_NR_IRQS)
+#define SX150X_EXP4_NR_IRQS	16
+#define SX150X_EXP4_NR_GPIOS	16
+
+#define SX150X_GPIO(_expander, _pin) (SX150X_EXP##_expander##_GPIO_BASE + _pin)
+
+enum {
+	SX150X_EPM,
+	SX150X_EXP1,
+	SX150X_EXP2,
+	SX150X_EXP3,
+	SX150X_EXP4,
+};
+
+
 #define MPQ8064_SPK_ON 1
 #define MPQ8064_SPK_OFF 0
 
@@ -147,6 +193,8 @@
 static struct snd_soc_jack hs_jack;
 static struct snd_soc_jack button_jack;
 
+static int detect_dtv_platform;
+
 static int msm_enable_codec_ext_clk(struct snd_soc_codec *codec, int enable,
 				    bool dapm);
 
@@ -763,6 +811,60 @@
 	return ret;
 }
 
+
+static int mpq_dtv_amp_power_up(void)
+{
+	int ret;
+	pr_debug("%s()\n", __func__);
+	ret = gpio_request(SX150X_GPIO(1, 14),
+				"DTV AMP Sleep");
+	if (ret) {
+		pr_err("%s: DTV AMP Sleep GPIO request returns %d\n",
+			   __func__, ret);
+		return ret;
+	}
+	ret = gpio_direction_output(SX150X_GPIO(1, 14), 0);
+	if (ret) {
+		pr_err("%s: DTV AMP Sleep GPIO set output returns %d\n",
+			   __func__, ret);
+		return ret;
+	}
+	ret = gpio_request(SX150X_GPIO(1, 13),
+				"DTV AMP Mute");
+	if (ret) {
+		pr_err("%s: DTV AMP Mute GPIO request returns %d\n",
+			   __func__, ret);
+		return ret;
+	}
+	ret = gpio_direction_output(SX150X_GPIO(1, 13), 0);
+	if (ret) {
+		pr_err("%s: DTV AMP Mute GPIO set output returns %d\n",
+			   __func__, ret);
+		return ret;
+	}
+	return ret;
+}
+
+static int mpq_dtv_amp_power_down(void)
+{
+	int ret;
+	pr_debug("%s()\n", __func__);
+	ret = gpio_direction_output(SX150X_GPIO(1, 14), 1);
+	if (ret) {
+		pr_err("%s: DTV AMP Sleep GPIO set output failed\n", __func__);
+		return ret;
+	}
+	gpio_free(SX150X_GPIO(1, 14));
+
+	ret = gpio_direction_output(SX150X_GPIO(1, 13), 1);
+	if (ret) {
+		pr_err("%s: DTV AMP Mute GPIO set output failed\n", __func__);
+		return ret;
+	}
+	gpio_free(SX150X_GPIO(1, 13));
+	return ret;
+}
+
 static int msm_audrx_init(struct snd_soc_pcm_runtime *rtd)
 {
 	int err;
@@ -810,6 +912,57 @@
 	snd_soc_dai_set_channel_map(codec_dai, ARRAY_SIZE(tx_ch),
 				    tx_ch, ARRAY_SIZE(rx_ch), rx_ch);
 
+	if (detect_dtv_platform) {
+		err = gpio_request(SX150X_GPIO(1, 11),
+				"DTV AMP Gain0");
+		if (err) {
+			pr_err("%s: DTV AMP Gain0 request returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		err = gpio_direction_output(SX150X_GPIO(1, 11), 0);
+		if (err) {
+			pr_err("%s: DTV AMP Gain0 set output returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		gpio_free(SX150X_GPIO(1, 11));
+
+		err = gpio_request(SX150X_GPIO(1, 12),
+				"DTV AMP Gain1");
+		if (err) {
+			pr_err("%s: DTV AMP Gain0 request returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		err = gpio_direction_output(SX150X_GPIO(1, 12), 0);
+		if (err) {
+			pr_err("%s: DTV AMP Gain1 set output returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		gpio_free(SX150X_GPIO(1, 12));
+
+		err = gpio_request(SX150X_GPIO(1, 15),
+				"DTV AMP Status");
+		if (err) {
+			pr_err("%s: DTV AMP Status request returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		err = gpio_direction_input(SX150X_GPIO(1, 15));
+		if (err) {
+			pr_err("%s: DTV AMP Status set output returns %d\n",
+				   __func__, err);
+			return err;
+		}
+		err = mpq_dtv_amp_power_down();
+		if (err) {
+			pr_err("%s: DTV AMP Status set output returns %d\n",
+				   __func__, err);
+			return err;
+		}
+	}
 	return err;
 }
 
@@ -1049,6 +1202,8 @@
 {
 	pr_debug("%s(): substream = %s  stream = %d\n", __func__,
 		 substream->name, substream->stream);
+	if (detect_dtv_platform)
+		mpq_dtv_amp_power_up();
 	return 0;
 }
 
@@ -1056,6 +1211,9 @@
 {
 	pr_debug("%s(): substream = %s  stream = %d\n", __func__,
 		 substream->name, substream->stream);
+
+	if (detect_dtv_platform)
+		mpq_dtv_amp_power_down();
 }
 
 static int mpq8064_auxpcm_startup(struct snd_pcm_substream *substream)
@@ -1670,7 +1828,9 @@
 
 	if (socinfo_get_pmic_model() == PMIC_MODEL_PM8917)
 		bottom_spk_pamp_gpio = PM8921_GPIO_PM_TO_SYS(16);
-
+	if (machine_is_mpq8064_dtv())
+		detect_dtv_platform = 1;
+	pr_info("MPQ8064: detect_dtv_platform is %d\n", detect_dtv_platform);
 	mbhc_cfg.calibration = def_tabla_mbhc_cal();
 	if (!mbhc_cfg.calibration) {
 		pr_err("Calibration data allocation failed\n");
diff --git a/sound/soc/msm/msm-dai-fe.c b/sound/soc/msm/msm-dai-fe.c
index 4165254..cf0d4cd 100644
--- a/sound/soc/msm/msm-dai-fe.c
+++ b/sound/soc/msm/msm-dai-fe.c
@@ -56,7 +56,7 @@
 					SNDRV_PCM_RATE_KNOT),
 			.formats = SNDRV_PCM_FMTBIT_S16_LE,
 			.channels_min = 1,
-			.channels_max = 6,
+			.channels_max = 8,
 			.rate_min =     8000,
 			.rate_max =	48000,
 		},
diff --git a/sound/soc/msm/msm-pcm-routing.c b/sound/soc/msm/msm-pcm-routing.c
index 79ce671..841d313 100644
--- a/sound/soc/msm/msm-pcm-routing.c
+++ b/sound/soc/msm/msm-pcm-routing.c
@@ -49,7 +49,8 @@
 
 static int fm_switch_enable;
 static int fm_pcmrx_switch_enable;
-static int srs_alsa_ctrl_ever_called;
+static short int srs_alsa_ctrl_ever_called_tm;
+static short int srs_alsa_ctrl_ever_called_ss3d;
 
 #define INT_RX_VOL_MAX_STEPS 0x2000
 #define INT_RX_VOL_GAIN 0x2000
@@ -125,19 +126,13 @@
 	unsigned short int raw_params[1];
 };
 static union srs_trumedia_params_u msm_srs_trumedia_params[2];
-static int srs_port_id = -1;
 
-static void srs_send_params(int port_id, unsigned int techs,
+static void srs_send_params_trumedia(int port_id, unsigned int techs,
 		int param_block_idx) {
-
-	/* only send commands to dsp if srs alsa ctrl was used
-	   at least one time */
-	if (!srs_alsa_ctrl_ever_called)
-		return;
-
 	pr_debug("SRS %s: called, port_id = %d, techs flags = %u,"
 			" paramblockidx %d", __func__, port_id, techs,
 			param_block_idx);
+
 	/* force all if techs is set to 1 */
 	if (techs == 1)
 		techs = 0xFFFFFFFF;
@@ -162,6 +157,46 @@
 	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.global);
 }
 
+union srs_SS3D_params_u {
+	struct srs_SS3D_params srs_params;
+	unsigned short int raw_params[1];
+};
+static union srs_SS3D_params_u msm_srs_SS3D_params[2];
+
+static void srs_send_params_SS3D(int port_id, unsigned int techs,
+					int param_block_idx) {
+	pr_debug("SRS %s: called, port_id = %d, techs flags = %u,\n"
+		" paramblockidx %d", __func__, port_id, techs,
+		param_block_idx);
+
+	/* force all if techs is set to 1 */
+	if (techs == 1)
+		techs = 0xFFFFFFFF;
+
+	if (techs & (1 << SRS_ID_SS3D_CTRL))
+		srs_ss3d_open(port_id, SRS_ID_SS3D_CTRL,
+		(void *)&msm_srs_SS3D_params[param_block_idx].srs_params.ss3d);
+	if (techs & (1 << SRS_ID_SS3D_FILTER))
+		srs_ss3d_open(port_id, SRS_ID_SS3D_FILTER,
+	    (void *)&msm_srs_SS3D_params[param_block_idx].srs_params.ss3d_f);
+	if (techs & (1 << SRS_ID_SS3D_GLOBAL))
+		srs_ss3d_open(port_id, SRS_ID_SS3D_GLOBAL,
+	(void *)&msm_srs_SS3D_params[param_block_idx].srs_params.global);
+	return;
+}
+
+static int srs_port_id = -1;
+static void srs_send_params(int port_id, unsigned int techs,
+				int param_block_id) {
+	if (srs_alsa_ctrl_ever_called_tm)
+		srs_send_params_trumedia(port_id, techs, param_block_id);
+	if (srs_alsa_ctrl_ever_called_ss3d)
+		srs_send_params_SS3D(port_id, techs, param_block_id);
+}
+
+/* This array is indexed by back-end DAI ID defined in msm-pcm-routing.h
+ * If new back-end is defined, add new back-end DAI ID at the end of enum
+ */
 static struct msm_pcm_routing_bdai_data msm_bedais[MSM_BACKEND_DAI_MAX] = {
 	{ PRIMARY_I2S_RX, 0, 0, 0, 0, 0},
 	{ PRIMARY_I2S_TX, 0, 0, 0, 0, 0},
@@ -994,7 +1029,7 @@
 	unsigned int techs = 0;
 	unsigned short offset, value, max, index;
 
-	srs_alsa_ctrl_ever_called = 1;
+	srs_alsa_ctrl_ever_called_tm = 1;
 
 	max = sizeof(msm_srs_trumedia_params) >> 1;
 	index = (unsigned short)((ucontrol->value.integer.value[0] &
@@ -1005,7 +1040,7 @@
 		pr_debug("SRS %s: send params request, flags = %u",
 			__func__, techs);
 		if (srs_port_id >= 0 && techs)
-			srs_send_params(srs_port_id, techs, index);
+			srs_send_params_trumedia(srs_port_id, techs, index);
 		return 0;
 	}
 	offset = (unsigned short)((ucontrol->value.integer.value[0] &
@@ -1014,32 +1049,10 @@
 			SRS_PARAM_VALUE_MASK);
 	if (offset < max) {
 		msm_srs_trumedia_params[index].raw_params[offset] = value;
-		pr_debug("SRS %s: index set... (max %d, requested %d,"
-			" val %d, paramblockidx %d)", __func__, max, offset,
-			value, index);
 	} else {
 		pr_err("SRS %s: index out of bounds! (max %d, requested %d)",
 				__func__, max, offset);
 	}
-	if (offset == 4) {
-		int i;
-		for (i = 0; i < max; i++) {
-			if (i == 0) {
-				pr_debug("SRS %s: global block start",
-						__func__);
-			}
-			if (i ==
-			(sizeof(struct srs_trumedia_params_GLOBAL) >> 1)) {
-				break;
-				pr_debug("SRS %s: wowhd block start at"
-					" offset %d word offset %d", __func__,
-					i, i>>1);
-			}
-			pr_debug("SRS %s: param_index %d index %d val %d",
-				__func__, index, i,
-				msm_srs_trumedia_params[index].raw_params[i]);
-		}
-	}
 	return 0;
 }
 
@@ -1047,7 +1060,6 @@
 				struct snd_ctl_elem_value *ucontrol) {
 	int ret;
 
-	pr_debug("SRS control normal called");
 	mutex_lock(&routing_lock);
 	srs_port_id = SLIMBUS_0_RX;
 	ret = msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
@@ -1060,7 +1072,6 @@
 		struct snd_ctl_elem_value *ucontrol) {
 	int ret;
 
-	pr_debug("SRS control I2S called");
 	mutex_lock(&routing_lock);
 	srs_port_id = PRIMARY_I2S_RX;
 	ret = msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
@@ -1073,7 +1084,6 @@
 		struct snd_ctl_elem_value *ucontrol) {
 	int ret;
 
-	pr_debug("SRS control HDMI called");
 	mutex_lock(&routing_lock);
 	srs_port_id = HDMI_RX;
 	ret =  msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
@@ -1081,6 +1091,83 @@
 	return ret;
 }
 
+static int msm_routing_get_srs_SS3D_control(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.integer.value[0] = 0;
+	return 0;
+}
+
+
+static int msm_routing_set_srs_SS3D_control_(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	unsigned int techs = 0;
+	unsigned short offset, value, max, index;
+
+	srs_alsa_ctrl_ever_called_ss3d = 1;
+
+	max = sizeof(msm_srs_SS3D_params) >> 1;
+	index = (unsigned short)((ucontrol->value.integer.value[0] &
+					 SRS_PARAM_INDEX_MASK) >> 31);
+	if (SRS_CMD_UPLOAD ==
+		 (ucontrol->value.integer.value[0] & SRS_CMD_UPLOAD)) {
+		techs = ucontrol->value.integer.value[0] & 0xFF;
+		pr_debug("SRS %s: send params request, flags = %u", __func__,
+			 techs);
+		if (srs_port_id >= 0 && techs)
+			srs_send_params_SS3D(srs_port_id, techs, index);
+		return 0;
+	}
+
+	offset = (unsigned short)((ucontrol->value.integer.value[0] &
+			SRS_PARAM_OFFSET_MASK) >> 16);
+	value = (unsigned short)(ucontrol->value.integer.value[0] &
+			 SRS_PARAM_VALUE_MASK);
+	if (offset < max) {
+		msm_srs_SS3D_params[index].raw_params[offset] = value;
+	} else {
+		pr_err("SRS %s: index out of bounds! (max %d, requested %d)",
+			 __func__, max, offset);
+	}
+	return 0;
+}
+
+static int msm_routing_set_srs_SS3D_control(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	mutex_lock(&routing_lock);
+	srs_port_id = SLIMBUS_0_RX;
+	ret = msm_routing_set_srs_SS3D_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
+static int msm_routing_set_srs_SS3D_control_I2S(
+		struct snd_kcontrol *kcontrol,
+		struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	mutex_lock(&routing_lock);
+	srs_port_id = PRIMARY_I2S_RX;
+	ret = msm_routing_set_srs_SS3D_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
+static int msm_routing_set_srs_SS3D_control_HDMI(
+		struct snd_kcontrol *kcontrol,
+		struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	mutex_lock(&routing_lock);
+	srs_port_id = HDMI_RX;
+	ret = msm_routing_set_srs_SS3D_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
 static void msm_send_eq_values(int eq_idx)
 {
 	int result;
@@ -2109,6 +2196,66 @@
 	}
 };
 
+static const struct snd_kcontrol_new lpa_SRS_SS3D_controls[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS SS3D",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+			SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_SS3D_control,
+	.put = msm_routing_set_srs_SS3D_control,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
+static const struct snd_kcontrol_new lpa_SRS_SS3D_controls_HDMI[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS SS3D HDMI",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+			SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_SS3D_control,
+	.put = msm_routing_set_srs_SS3D_control_HDMI,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
+static const struct snd_kcontrol_new lpa_SRS_SS3D_controls_I2S[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS SS3D I2S",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+		SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_SS3D_control,
+	.put = msm_routing_set_srs_SS3D_control_I2S,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
 static const struct snd_kcontrol_new eq_enable_mixer_controls[] = {
 	SOC_SINGLE_EXT("MultiMedia1 EQ Enable", SND_SOC_NOPM,
 	MSM_FRONTEND_DAI_MULTIMEDIA1, 1, 0, msm_routing_get_eq_enable_mixer,
@@ -3060,6 +3207,18 @@
 			ARRAY_SIZE(lpa_SRS_trumedia_controls_I2S));
 
 	snd_soc_add_platform_controls(platform,
+				lpa_SRS_SS3D_controls,
+			ARRAY_SIZE(lpa_SRS_SS3D_controls));
+
+	snd_soc_add_platform_controls(platform,
+				lpa_SRS_SS3D_controls_HDMI,
+			ARRAY_SIZE(lpa_SRS_SS3D_controls_HDMI));
+
+	snd_soc_add_platform_controls(platform,
+				lpa_SRS_SS3D_controls_I2S,
+			ARRAY_SIZE(lpa_SRS_SS3D_controls_I2S));
+
+	snd_soc_add_platform_controls(platform,
 				ec_ref_rx_mixer_controls,
 			ARRAY_SIZE(ec_ref_rx_mixer_controls));
 
diff --git a/sound/soc/msm/msm8930.c b/sound/soc/msm/msm8930.c
index 42699c9..bb9f2be 100644
--- a/sound/soc/msm/msm8930.c
+++ b/sound/soc/msm/msm8930.c
@@ -58,7 +58,7 @@
 static int msm8930_ext_spk_pamp;
 static int msm8930_btsco_rate = BTSCO_RATE_8KHZ;
 static int msm8930_btsco_ch = 1;
-
+static int hdmi_rate_variable;
 static struct clk *codec_clk;
 static int clk_users;
 
@@ -395,10 +395,13 @@
 static const char *slim0_rx_ch_text[] = {"One", "Two"};
 static const char *slim0_tx_ch_text[] = {"One", "Two", "Three", "Four"};
 
+static const char * const hdmi_rate[] = {"Default", "Variable"};
+
 static const struct soc_enum msm8930_enum[] = {
 	SOC_ENUM_SINGLE_EXT(2, spk_function),
 	SOC_ENUM_SINGLE_EXT(2, slim0_rx_ch_text),
 	SOC_ENUM_SINGLE_EXT(4, slim0_tx_ch_text),
+	SOC_ENUM_SINGLE_EXT(2, hdmi_rate),
 };
 
 static const char *btsco_rate_text[] = {"8000", "16000"};
@@ -505,6 +508,21 @@
 	return ret;
 }
 
+static int msm8930_hdmi_rate_put(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	hdmi_rate_variable = ucontrol->value.integer.value[0];
+	pr_debug("%s: hdmi_rate_variable = %d\n", __func__, hdmi_rate_variable);
+	return 0;
+}
+
+static int msm8930_hdmi_rate_get(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.integer.value[0] = hdmi_rate_variable;
+	return 0;
+}
+
 static const struct snd_kcontrol_new sitar_msm8930_controls[] = {
 	SOC_ENUM_EXT("Speaker Function", msm8930_enum[0], msm8930_get_spk,
 		msm8930_set_spk),
@@ -516,6 +534,9 @@
 		msm8930_pmic_gain_get, msm8930_pmic_gain_put),
 	SOC_ENUM_EXT("Internal BTSCO SampleRate", msm8930_btsco_enum[0],
 		msm8930_btsco_rate_get, msm8930_btsco_rate_put),
+	SOC_ENUM_EXT("HDMI RX Rate", msm8930_enum[3],
+					msm8930_hdmi_rate_get,
+					msm8930_hdmi_rate_put),
 };
 
 static void *def_sitar_mbhc_cal(void)
@@ -751,7 +772,8 @@
 	struct snd_interval *channels = hw_param_interval(params,
 					SNDRV_PCM_HW_PARAM_CHANNELS);
 
-	rate->min = rate->max = 48000;
+	if (!hdmi_rate_variable)
+		rate->min = rate->max = 48000;
 	channels->min = channels->max = 2;
 
 	return 0;
diff --git a/sound/soc/msm/msm8960.c b/sound/soc/msm/msm8960.c
index ad78255..da62729 100644
--- a/sound/soc/msm/msm8960.c
+++ b/sound/soc/msm/msm8960.c
@@ -73,7 +73,7 @@
 
 static int msm8960_btsco_rate = SAMPLE_RATE_8KHZ;
 static int msm8960_btsco_ch = 1;
-
+static int hdmi_rate_variable;
 static int msm8960_auxpcm_rate = SAMPLE_RATE_8KHZ;
 
 static struct clk *codec_clk;
@@ -549,11 +549,13 @@
 static const char *spk_function[] = {"Off", "On"};
 static const char *slim0_rx_ch_text[] = {"One", "Two"};
 static const char *slim0_tx_ch_text[] = {"One", "Two", "Three", "Four"};
+static const char * const hdmi_rate[] = {"Default", "Variable"};
 
 static const struct soc_enum msm8960_enum[] = {
 	SOC_ENUM_SINGLE_EXT(2, spk_function),
 	SOC_ENUM_SINGLE_EXT(2, slim0_rx_ch_text),
 	SOC_ENUM_SINGLE_EXT(4, slim0_tx_ch_text),
+	SOC_ENUM_SINGLE_EXT(2, hdmi_rate),
 };
 
 static const char *btsco_rate_text[] = {"8000", "16000"};
@@ -660,6 +662,21 @@
 	return 0;
 }
 
+static int msm8960_hdmi_rate_put(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	hdmi_rate_variable = ucontrol->value.integer.value[0];
+	pr_debug("%s: hdmi_rate_variable = %d\n", __func__, hdmi_rate_variable);
+	return 0;
+}
+
+static int msm8960_hdmi_rate_get(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.integer.value[0] = hdmi_rate_variable;
+	return 0;
+}
+
 static const struct snd_kcontrol_new tabla_msm8960_controls[] = {
 	SOC_ENUM_EXT("Speaker Function", msm8960_enum[0], msm8960_get_spk,
 		msm8960_set_spk),
@@ -671,6 +688,9 @@
 		msm8960_btsco_rate_get, msm8960_btsco_rate_put),
 	SOC_ENUM_EXT("AUX PCM SampleRate", msm8960_auxpcm_enum[0],
 		msm8960_auxpcm_rate_get, msm8960_auxpcm_rate_put),
+	SOC_ENUM_EXT("HDMI RX Rate", msm8960_enum[3],
+					msm8960_hdmi_rate_get,
+					msm8960_hdmi_rate_put),
 };
 
 static void *def_tabla_mbhc_cal(void)
@@ -1003,7 +1023,8 @@
 
 	if (channels->max < 2)
 		channels->min = channels->max = 2;
-	rate->min = rate->max = 48000;
+	if (!hdmi_rate_variable)
+		rate->min = rate->max = 48000;
 
 	return 0;
 }
diff --git a/sound/soc/msm/msm8974.c b/sound/soc/msm/msm8974.c
index f8185bb..633066f 100644
--- a/sound/soc/msm/msm8974.c
+++ b/sound/soc/msm/msm8974.c
@@ -876,7 +876,7 @@
 };
 
 /* Digital audio interface glue - connects codec <---> CPU */
-static struct snd_soc_dai_link msm8974_dai[] = {
+static struct snd_soc_dai_link msm8974_common_dai_links[] = {
 	/* FrontEnd DAI Links */
 	{
 		.name = "MSM8974 Media1",
@@ -1181,19 +1181,6 @@
 		.codec_dai_name = "snd-soc-dummy-dai",
 		.codec_name = "snd-soc-dummy",
 	},
-	/* HDMI BACK END DAI Link */
-	{
-		.name = LPASS_BE_HDMI,
-		.stream_name = "HDMI Playback",
-		.cpu_dai_name = "msm-dai-q6-hdmi.8",
-		.platform_name = "msm-pcm-routing",
-		.codec_name     = "msm-stub-codec.1",
-		.codec_dai_name = "msm-stub-rx",
-		.no_pcm = 1,
-		.be_id = MSM_BACKEND_DAI_HDMI_RX,
-		.be_hw_params_fixup = msm8974_hdmi_be_hw_params_fixup,
-		.ignore_pmdown_time = 1,
-	},
 	/* AUX PCM Backend DAI Links */
 	{
 		.name = LPASS_BE_AUXPCM_RX,
@@ -1326,12 +1313,54 @@
 		.be_hw_params_fixup = msm_slim_0_tx_be_hw_params_fixup,
 		.ops = &msm8974_be_ops,
 	},
+	/* Incall Record Uplink BACK END DAI Link */
+	{
+		.name = LPASS_BE_INCALL_RECORD_TX,
+		.stream_name = "Voice Uplink Capture",
+		.cpu_dai_name = "msm-dai-q6-dev.32772",
+		.platform_name = "msm-pcm-routing",
+		.codec_name     = "msm-stub-codec.1",
+		.codec_dai_name = "msm-stub-tx",
+		.no_pcm = 1,
+		.be_id = MSM_BACKEND_DAI_INCALL_RECORD_TX,
+		.be_hw_params_fixup = msm_be_hw_params_fixup,
+	},
+	/* Incall Record Downlink BACK END DAI Link */
+	{
+		.name = LPASS_BE_INCALL_RECORD_RX,
+		.stream_name = "Voice Downlink Capture",
+		.cpu_dai_name = "msm-dai-q6-dev.32771",
+		.platform_name = "msm-pcm-routing",
+		.codec_name     = "msm-stub-codec.1",
+		.codec_dai_name = "msm-stub-tx",
+		.no_pcm = 1,
+		.be_id = MSM_BACKEND_DAI_INCALL_RECORD_RX,
+		.be_hw_params_fixup = msm_be_hw_params_fixup,
+	},
 };
 
+static struct snd_soc_dai_link msm8974_hdmi_dai_link[] = {
+/* HDMI BACK END DAI Link */
+	{
+		.name = LPASS_BE_HDMI,
+		.stream_name = "HDMI Playback",
+		.cpu_dai_name = "msm-dai-q6-hdmi.8",
+		.platform_name = "msm-pcm-routing",
+		.codec_name     = "msm-hdmi-audio-codec-rx",
+		.codec_dai_name = "msm_hdmi_audio_codec_rx_dai",
+		.no_pcm = 1,
+		.be_id = MSM_BACKEND_DAI_HDMI_RX,
+		.be_hw_params_fixup = msm8974_hdmi_be_hw_params_fixup,
+		.ignore_pmdown_time = 1,
+	},
+};
+
+static struct snd_soc_dai_link msm8974_dai_links[
+					 ARRAY_SIZE(msm8974_common_dai_links) +
+					 ARRAY_SIZE(msm8974_hdmi_dai_link)];
+
 struct snd_soc_card snd_soc_card_msm8974 = {
 	.name		= "msm8974-taiko-snd-card",
-	.dai_link	= msm8974_dai,
-	.num_links	= ARRAY_SIZE(msm8974_dai),
 };
 
 static int msm8974_prepare_codec_mclk(struct snd_soc_card *card)
@@ -1428,6 +1457,25 @@
 	if (ret)
 		goto err;
 
+	if (of_property_read_bool(pdev->dev.of_node, "qcom,hdmi-audio-rx")) {
+		dev_info(&pdev->dev, "%s(): hdmi audio support present\n",
+				__func__);
+
+		memcpy(msm8974_dai_links, msm8974_common_dai_links,
+			sizeof(msm8974_common_dai_links));
+
+		memcpy(msm8974_dai_links + ARRAY_SIZE(msm8974_common_dai_links),
+			msm8974_hdmi_dai_link, sizeof(msm8974_hdmi_dai_link));
+
+		card->dai_link	= msm8974_dai_links;
+		card->num_links	= ARRAY_SIZE(msm8974_dai_links);
+	} else {
+		dev_info(&pdev->dev, "%s(): No hdmi audio support\n", __func__);
+
+		card->dai_link	= msm8974_common_dai_links;
+		card->num_links	= ARRAY_SIZE(msm8974_common_dai_links);
+	}
+
 	ret = snd_soc_register_card(card);
 	if (ret) {
 		dev_err(&pdev->dev, "snd_soc_register_card failed (%d)\n",
diff --git a/sound/soc/msm/qdsp6/q6adm.c b/sound/soc/msm/qdsp6/q6adm.c
index 119e017..2d8d9ca 100644
--- a/sound/soc/msm/qdsp6/q6adm.c
+++ b/sound/soc/msm/qdsp6/q6adm.c
@@ -230,6 +230,182 @@
 	return ret;
 }
 
+struct SS3D {
+	int     _1;  int     _2;  short   _3;  short   _4;
+	short   _5;  short   _6;  int     _7;  int     _X[32];
+	short   _8;  short   _9;  short   _10; short   _11;
+	short   _12; short   _13; short   _14; short   _15;
+	short   _16; short   _17; short   _18; short	_19;
+	short   _20; short   _21; short   _22; short   _23;
+	short   _24; short   _25; short   _26[5];
+	short   _27; short   _28; short	  _29; short   _30;
+	short   _31; short   _32; short   _33; int     _34; int   _35;
+	int     _36; int     _37; int     _38; int     _39; int   _40;
+};
+
+struct SS3D_F {
+	int	_1; int _2; int _3; int _4; int _5; int _6; int _7; int _X[];
+};
+
+int srs_ss3d_open(int port_id, int srs_tech_id, void *srs_params)
+{
+	struct asm_pp_params_command *open = NULL;
+	int ret = 0, sz = 0;
+
+	int index;
+
+	pr_debug("SRS - %s: called.", __func__);
+
+	switch (srs_tech_id) {
+	case SRS_ID_SS3D_GLOBAL: {
+		struct srs_SS3D_params_GLOBAL *glb_params = NULL;
+		sz = sizeof(struct asm_pp_params_command) +
+			 sizeof(struct srs_SS3D_params_GLOBAL);
+		open = kzalloc(sz, GFP_KERNEL);
+
+		open->payload_size = sizeof(struct srs_SS3D_params_GLOBAL) +
+					 sizeof(struct asm_pp_param_data_hdr);
+		open->params.param_id = SRS_SS3D_PARAMS;
+		open->params.param_size =
+				 sizeof(struct srs_SS3D_params_GLOBAL);
+
+		glb_params = (struct srs_SS3D_params_GLOBAL *)((u8 *)open +
+				 sizeof(struct asm_pp_params_command));
+		memcpy(glb_params, srs_params,
+				 sizeof(struct srs_SS3D_params_GLOBAL));
+
+		pr_debug("SRS - ss3d global params - 1 = %x, 2 = %x, 3 = %x\n"
+			" 4 = %x, 5 = %x, 6 = %x, 7 = %x, 8 = %x\n",
+			(int)glb_params->v1, (int)glb_params->v2,
+			(int)glb_params->v3, (int)glb_params->v4,
+			(int)glb_params->v5, (int)glb_params->v6,
+			(int)glb_params->v7, (int)glb_params->v8);
+		break;
+	}
+	case SRS_ID_SS3D_CTRL: {
+		struct srs_SS3D_ctrl_params *whd_params = NULL;
+		sz = sizeof(struct asm_pp_params_command) +
+			 sizeof(struct srs_SS3D_ctrl_params);
+		open = kzalloc(sz, GFP_KERNEL);
+
+		open->payload_size = sizeof(struct srs_SS3D_ctrl_params) +
+			 sizeof(struct asm_pp_param_data_hdr);
+		open->params.param_id = SRS_SS3D_PARAMS_CTRL;
+		open->params.param_size = sizeof(struct srs_SS3D_ctrl_params);
+
+		whd_params = (struct srs_SS3D_ctrl_params *)((u8 *)open +
+				 sizeof(struct asm_pp_params_command));
+		memcpy(whd_params, srs_params,
+			 sizeof(struct srs_SS3D_ctrl_params));
+
+		{
+			struct SS3D *D = (struct SS3D *)whd_params->v;
+			pr_debug("SRS - ss3d ctrl params\n"
+				"1 = 0x%08X, 2 = 0x%08X, 3 = 0x%04X,\n"
+				"4 = 0x%04X, 5 = 0x%04X, 6 = 0x%04X,\n"
+				"7 = 0x%08X, 8 = 0x%04X, 9 = 0x%04X,\n"
+				"10 = 0x%04X, 11 = 0x%04X, 12 = 0x%04X,\n"
+				"13 = 0x%04X, 14 = 0x%04X, 15 = 0x%04X,\n"
+				"16 = 0x%04X, 17 = 0x%04X, 18 = 0x%04X,\n"
+				"19 = 0x%04X, 20 = 0x%04X, 21 = 0x%04X,\n"
+				"22 = 0x%04X, 23 = 0x%04X, 24 = 0x%04X,\n"
+				"25 = 0x%04X, 26.0 = 0x%04X, 26.1 = 0x%04X,\n"
+				"26.2 = 0x%04X, 26.3 = 0x%04X,\n"
+				"26.4 = 0x%04X, 27 = 0x%04X, 28 = 0x%04X,\n"
+				"29 = 0x%04X, 30 = 0x%04X, 31 = 0x%04X,\n"
+				"32 = 0x%04X, 33 = 0x%04X, 34 = 0x%08X,\n"
+				"35 = 0x%08X, 36 = 0x%08X, 37 = 0x%08X,\n"
+				"38 = 0x%08X, 39 = 0x%08X, 40 = 0x%08X",
+				D->_1, D->_2, D->_3, D->_4, D->_5, D->_6, D->_7,
+				D->_8, D->_9, D->_10, D->_11, D->_12, D->_13,
+				D->_14, D->_15, D->_16, D->_17, D->_18, D->_19,
+				D->_20,	D->_21, D->_22,	D->_23, D->_24, D->_25,
+				D->_26[0], D->_26[1], D->_26[2], D->_26[3],
+				D->_26[4], D->_27, D->_28, D->_29, D->_30,
+				D->_31, D->_32, D->_33, D->_34, D->_35, D->_36,
+				D->_37,	D->_38, D->_39, D->_40);
+		}
+		break;
+	}
+	case SRS_ID_SS3D_FILTER: {
+		struct srs_SS3D_filter_params *chp_params = NULL;
+		sz = sizeof(struct asm_pp_params_command) +
+			 sizeof(struct srs_SS3D_filter_params);
+		open = kzalloc(sz, GFP_KERNEL);
+
+		open->payload_size = sizeof(struct srs_SS3D_filter_params) +
+					sizeof(struct asm_pp_param_data_hdr);
+		open->params.param_id = SRS_SS3D_PARAMS_FILTER;
+		open->params.param_size =
+				 sizeof(struct srs_SS3D_filter_params);
+
+		chp_params = (struct srs_SS3D_filter_params *)((u8 *)open +
+				 sizeof(struct asm_pp_params_command));
+		memcpy(chp_params, srs_params,
+			 sizeof(struct srs_SS3D_filter_params));
+
+		{
+			struct SS3D_F *D = (struct SS3D_F *)chp_params->v;
+			pr_debug("SRS - ss3d filter params\n"
+				"1 = 0x%08X, 2 = 0x%08X, 3 = 0x%08X\n"
+				"4 = 0x%08X, 5 = 0x%08X, 6 = 0x%08X\n"
+				"7 = 0x%08X", D->_1, D->_2, D->_3, D->_4, D->_5,
+				D->_6, D->_7);
+		}
+		break;
+	}
+	default:
+		pr_debug("SRS - bad param!\n");
+		goto fail_cmd;
+	}
+
+	open->payload = NULL;
+
+	open->params.module_id = SRS_SS3D_MODULE_ID;
+	open->params.reserved = 0;
+
+	open->hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+				 APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	open->hdr.pkt_size = sz;
+	open->hdr.src_svc = APR_SVC_ADM;
+	open->hdr.src_domain = APR_DOMAIN_APPS;
+	open->hdr.src_port = port_id;
+	open->hdr.dest_svc = APR_SVC_ADM;
+	open->hdr.dest_domain = APR_DOMAIN_ADSP;
+
+	index = afe_get_port_index(port_id);
+	open->hdr.dest_port = atomic_read(&this_adm.copp_id[index]);
+		 /* port_id;//atomic_read(&this_adm.copp_id[port_id]); */
+	open->hdr.token = port_id;
+	open->hdr.opcode = ADM_CMD_SET_PARAMS;
+
+	pr_debug("SRS - %s: Command was sent now check Q6 - port id = %d,\n"
+		"size %d, module id %x, param id %x.\n",
+		 __func__, open->hdr.dest_port, open->payload_size,
+		 open->params.module_id, open->params.param_id);
+
+	ret = apr_send_pkt(this_adm.apr, (uint32_t *)open);
+	if (ret < 0) {
+		pr_err("SRS - %s: ADM enable for port %d failed\n",
+			 __func__, port_id);
+		ret = -EINVAL;
+		goto fail_cmd;
+	}
+	/* Wait for the callback with copp id */
+	ret = wait_event_timeout(this_adm.wait, 1,
+		 msecs_to_jiffies(TIMEOUT_MS));
+	if (!ret) {
+		pr_err("SRS - %s: ADM open failed for port %d\n",
+			 __func__, port_id);
+		ret = -EINVAL;
+		goto fail_cmd;
+	}
+
+fail_cmd:
+	kfree(open);
+	return ret;
+}
+
 static int32_t adm_callback(struct apr_client_data *data, void *priv)
 {
 	uint32_t *payload;
diff --git a/sound/soc/msm/qdsp6v2/msm-compr-q6-v2.c b/sound/soc/msm/qdsp6v2/msm-compr-q6-v2.c
index 94c1c85..8fb70f8 100644
--- a/sound/soc/msm/qdsp6v2/msm-compr-q6-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-compr-q6-v2.c
@@ -87,11 +87,11 @@
 	.rate_max =	     48000,
 	.channels_min =	 1,
 	.channels_max =	 8,
-	.buffer_bytes_max =     1200 * 1024 * 2,
-	.period_bytes_min =	2400,
-	.period_bytes_max =     1200 * 1024,
-	.periods_min =	  2,
-	.periods_max =	  1024,
+	.buffer_bytes_max =     1024 * 1024,
+	.period_bytes_min =	128 * 1024,
+	.period_bytes_max =     256 * 1024,
+	.periods_min =	  4,
+	.periods_max =	  8,
 	.fifo_size =	    0,
 };
 
diff --git a/sound/soc/msm/qdsp6v2/msm-dai-q6-hdmi-v2.c b/sound/soc/msm/qdsp6v2/msm-dai-q6-hdmi-v2.c
index f80b5891..48d9e1e 100644
--- a/sound/soc/msm/qdsp6v2/msm-dai-q6-hdmi-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-dai-q6-hdmi-v2.c
@@ -23,8 +23,6 @@
 #include <sound/apr_audio-v2.h>
 #include <sound/q6afe-v2.h>
 #include <sound/msm-dai-q6-v2.h>
-#include <mach/msm_hdmi_audio.h>
-
 
 enum {
 	STATUS_PORT_STARTED, /* track if AFE port has started */
@@ -88,9 +86,6 @@
 				struct snd_soc_dai *dai)
 {
 	struct msm_dai_q6_hdmi_dai_data *dai_data = dev_get_drvdata(dai->dev);
-	u32 channel_allocation = 0;
-	u32 level_shift  = 0; /* 0dB */
-	bool down_mix = FALSE;
 
 	dai_data->channels = params_channels(params);
 	dai_data->rate = params_rate(params);
@@ -101,33 +96,21 @@
 
 	switch (dai_data->channels) {
 	case 2:
-		channel_allocation  = 0;
-		hdmi_msm_audio_info_setup(1, MSM_HDMI_AUDIO_CHANNEL_2,
-				channel_allocation, level_shift, down_mix);
-		dai_data->port_config.hdmi_multi_ch.channel_allocation =
-			channel_allocation;
+		dai_data->port_config.hdmi_multi_ch.channel_allocation = 0;
 		break;
 	case 6:
-		channel_allocation  = 0x0B;
-		hdmi_msm_audio_info_setup(1, MSM_HDMI_AUDIO_CHANNEL_6,
-				channel_allocation, level_shift, down_mix);
-		dai_data->port_config.hdmi_multi_ch.channel_allocation =
-				channel_allocation;
+		dai_data->port_config.hdmi_multi_ch.channel_allocation = 0x0B;
 		break;
 	case 8:
-		channel_allocation  = 0x1F;
-		hdmi_msm_audio_info_setup(1, MSM_HDMI_AUDIO_CHANNEL_8,
-				channel_allocation, level_shift, down_mix);
-		dai_data->port_config.hdmi_multi_ch.channel_allocation =
-				channel_allocation;
+		dai_data->port_config.hdmi_multi_ch.channel_allocation = 0x13;
 		break;
 	default:
 		dev_err(dai->dev, "invalid Channels = %u\n",
 				dai_data->channels);
 		return -EINVAL;
 	}
-	dev_dbg(dai->dev, "%s() minor version: %u samplerate: %u bitwidth: %u num_ch = %u channel_allocation = %u datatype = %d\n",
-		 __func__,
+	dev_dbg(dai->dev, "%s() minor version: %u samplerate: %u bitwidth: %u\n"
+		"num_ch = %u channel_allocation = %u datatype = %d\n", __func__,
 		dai_data->port_config.hdmi_multi_ch.hdmi_cfg_minor_version,
 		dai_data->port_config.hdmi_multi_ch.sample_rate,
 		dai_data->port_config.hdmi_multi_ch.bit_width,
@@ -239,7 +222,7 @@
 		.rates = SNDRV_PCM_RATE_48000,
 		.formats = SNDRV_PCM_FMTBIT_S16_LE,
 		.channels_min = 2,
-		.channels_max = 6,
+		.channels_max = 8,
 		.rate_max =     48000,
 		.rate_min =	48000,
 	},
diff --git a/sound/soc/msm/qdsp6v2/msm-dai-q6-v2.c b/sound/soc/msm/qdsp6v2/msm-dai-q6-v2.c
index a307bcc..2f4c256 100644
--- a/sound/soc/msm/qdsp6v2/msm-dai-q6-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-dai-q6-v2.c
@@ -432,12 +432,11 @@
 	if (!test_bit(STATUS_PORT_STARTED, dai_data->status_mask)) {
 		switch (dai->id) {
 		case VOICE_PLAYBACK_TX:
-		case VOICE_RECORD_TX:
-		case VOICE_RECORD_RX:
 			rc = afe_start_pseudo_port(dai->id);
+			break;
 		default:
 			rc = afe_port_start(dai->id, &dai_data->port_config,
-					    dai_data->rate);
+					dai_data->rate);
 		}
 
 		if (IS_ERR_VALUE(rc))
@@ -606,6 +605,36 @@
 	return 0;
 }
 
+static int msm_dai_q6_psuedo_port_hw_params(struct snd_pcm_hw_params *params,
+				    struct snd_soc_dai *dai, int stream)
+{
+	struct msm_dai_q6_dai_data *dai_data = dev_get_drvdata(dai->dev);
+
+	dai_data->channels = params_channels(params);
+	dai_data->rate = params_rate(params);
+
+	/* Q6 only supports 16 as now */
+	dai_data->port_config.pseudo_port.pseud_port_cfg_minor_version =
+				AFE_API_VERSION_PSEUDO_PORT_CONFIG;
+	dai_data->port_config.pseudo_port.num_channels =
+				params_channels(params);
+	dai_data->port_config.pseudo_port.bit_width = 16;
+	dai_data->port_config.pseudo_port.data_format = 0;
+	dai_data->port_config.pseudo_port.timing_mode =
+				AFE_PSEUDOPORT_TIMING_MODE_TIMER;
+	dai_data->port_config.pseudo_port.sample_rate = params_rate(params);
+
+	dev_dbg(dai->dev, "%s: bit_wd[%hu] num_channels [%hu] format[%hu]\n"
+		"timing Mode %hu sample_rate %d\n", __func__,
+		dai_data->port_config.pseudo_port.bit_width,
+		dai_data->port_config.pseudo_port.num_channels,
+		dai_data->port_config.pseudo_port.data_format,
+		dai_data->port_config.pseudo_port.timing_mode,
+		dai_data->port_config.pseudo_port.sample_rate);
+
+	return 0;
+}
+
 /* Current implementation assumes hw_param is called once
  * This may not be the case but what to do when ADM and AFE
  * port are already opened and parameter changes
@@ -649,9 +678,12 @@
 		rc = msm_dai_q6_afe_rtproxy_hw_params(params, dai);
 		break;
 	case VOICE_PLAYBACK_TX:
+		rc = 0;
+		break;
 	case VOICE_RECORD_RX:
 	case VOICE_RECORD_TX:
-		rc = 0;
+		rc = msm_dai_q6_psuedo_port_hw_params(params,
+						dai, substream->stream);
 		break;
 	default:
 		dev_err(dai->dev, "invalid AFE port ID\n");
@@ -671,8 +703,6 @@
 	if (test_bit(STATUS_PORT_STARTED, dai_data->status_mask)) {
 		switch (dai->id) {
 		case VOICE_PLAYBACK_TX:
-		case VOICE_RECORD_TX:
-		case VOICE_RECORD_RX:
 			pr_debug("%s, stop pseudo port:%d\n",
 						__func__,  dai->id);
 			rc = afe_stop_pseudo_port(dai->id);
@@ -832,8 +862,6 @@
 	if (test_bit(STATUS_PORT_STARTED, dai_data->status_mask)) {
 		switch (dai->id) {
 		case VOICE_PLAYBACK_TX:
-		case VOICE_RECORD_TX:
-		case VOICE_RECORD_RX:
 			pr_debug("%s, stop pseudo port:%d\n",
 				 __func__,  dai->id);
 			rc = afe_stop_pseudo_port(dai->id);
@@ -872,7 +900,7 @@
 		SNDRV_PCM_RATE_16000,
 		.formats = SNDRV_PCM_FMTBIT_S16_LE,
 		.channels_min = 1,
-		.channels_max = 4,
+		.channels_max = 8,
 		.rate_min =     8000,
 		.rate_max =	48000,
 	},
@@ -969,6 +997,21 @@
 	.remove = msm_dai_q6_dai_remove,
 };
 
+static struct snd_soc_dai_driver msm_dai_q6_incall_record_dai = {
+	.capture = {
+		.rates = SNDRV_PCM_RATE_48000 | SNDRV_PCM_RATE_8000 |
+		SNDRV_PCM_RATE_16000,
+		.formats = SNDRV_PCM_FMTBIT_S16_LE,
+		.channels_min = 1,
+		.channels_max = 2,
+		.rate_min =     8000,
+		.rate_max =     48000,
+	},
+	.ops = &msm_dai_q6_ops,
+	.probe = msm_dai_q6_dai_probe,
+	.remove = msm_dai_q6_dai_remove,
+};
+
 static int __devinit msm_auxpcm_dev_probe(struct platform_device *pdev)
 {
 	int id;
@@ -1879,6 +1922,12 @@
 	case RT_PROXY_DAI_002_TX:
 		rc = snd_soc_register_dai(&pdev->dev, &msm_dai_q6_afe_tx_dai);
 		break;
+	case VOICE_RECORD_RX:
+	case VOICE_RECORD_TX:
+		rc = snd_soc_register_dai(&pdev->dev,
+						&msm_dai_q6_incall_record_dai);
+		break;
+
 	default:
 		rc = -ENODEV;
 		break;
diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-afe-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-afe-v2.c
index 73a04c2..1aa12e3 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-afe-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-afe-v2.c
@@ -33,8 +33,10 @@
 #include <mach/msm_subsystem_map.h>
 #include "msm-pcm-afe-v2.h"
 
-#define MIN_PERIOD_SIZE (128 * 2)
-#define MAX_PERIOD_SIZE (128 * 2 * 2 * 6)
+#define MIN_PERIOD_SIZE (128 * 2 * 8)
+#define MAX_PERIOD_SIZE (128 * 2 * 2 * 6 * 8)
+#define MAX_NUM_PERIODS 384
+#define MIN_NUM_PERIODS 32
 static struct snd_pcm_hardware msm_afe_hardware = {
 	.info =			(SNDRV_PCM_INFO_MMAP |
 				SNDRV_PCM_INFO_BLOCK_TRANSFER |
@@ -47,12 +49,12 @@
 	.rate_min =             8000,
 	.rate_max =             48000,
 	.channels_min =         1,
-	.channels_max =         2,
+	.channels_max =         8,
 	.buffer_bytes_max =     MAX_PERIOD_SIZE * 32,
 	.period_bytes_min =     MIN_PERIOD_SIZE,
 	.period_bytes_max =     MAX_PERIOD_SIZE,
-	.periods_min =          32,
-	.periods_max =          384,
+	.periods_min =          MIN_NUM_PERIODS,
+	.periods_max =          MAX_NUM_PERIODS,
 	.fifo_size =            0,
 };
 static enum hrtimer_restart afe_hrtimer_callback(struct hrtimer *hrt);
@@ -353,6 +355,17 @@
 	if (ret < 0)
 		pr_err("snd_pcm_hw_constraint_integer failed\n");
 
+	if (substream->stream == SNDRV_PCM_STREAM_CAPTURE) {
+		ret = snd_pcm_hw_constraint_minmax(runtime,
+			SNDRV_PCM_HW_PARAM_BUFFER_BYTES,
+			MAX_NUM_PERIODS * MIN_PERIOD_SIZE,
+			MIN_NUM_PERIODS * MAX_PERIOD_SIZE);
+		if (ret < 0) {
+			pr_err("constraint for buffer bytes min max ret = %d\n",
+									ret);
+		}
+	}
+
 	return 0;
 }
 
@@ -497,8 +510,8 @@
 		dir = OUT;
 	rc = q6afe_audio_client_buf_alloc_contiguous(dir,
 			prtd->audio_client,
-			runtime->hw.period_bytes_min,
-			runtime->hw.periods_max);
+			(params_buffer_bytes(params) / params_periods(params)),
+			params_periods(params));
 	if (rc < 0) {
 		pr_err("Audio Start: Buffer Allocation failed rc = %d\n", rc);
 		mutex_unlock(&prtd->lock);
@@ -517,14 +530,14 @@
 	dma_buf->private_data = NULL;
 	dma_buf->area = buf[0].data;
 	dma_buf->addr = buf[0].phys;
-	dma_buf->bytes = runtime->hw.buffer_bytes_max;
+	dma_buf->bytes = params_buffer_bytes(params);
 	if (!dma_buf->area) {
 		pr_err("%s:MSM AFE physical memory allocation failed\n",
 							__func__);
 		mutex_unlock(&prtd->lock);
 		return -ENOMEM;
 	}
-	memset(dma_buf->area, 0, runtime->hw.buffer_bytes_max);
+	memset(dma_buf->area, 0, params_buffer_bytes(params));
 	prtd->dma_addr = (u32) dma_buf->addr;
 
 	mutex_unlock(&prtd->lock);
diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
index 1e6fc04..1d6e106 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-q6-v2.c
@@ -41,7 +41,8 @@
 };
 
 #define PLAYBACK_NUM_PERIODS	8
-#define PLAYBACK_PERIOD_SIZE	2048
+#define PLAYBACK_MAX_PERIOD_SIZE    12288
+#define PLAYBACK_MIN_PERIOD_SIZE    2048
 #define CAPTURE_NUM_PERIODS	16
 #define CAPTURE_PERIOD_SIZE	512
 
@@ -76,10 +77,10 @@
 	.rate_min =             8000,
 	.rate_max =             48000,
 	.channels_min =         1,
-	.channels_max =         2,
-	.buffer_bytes_max =     PLAYBACK_NUM_PERIODS * PLAYBACK_PERIOD_SIZE,
-	.period_bytes_min =	PLAYBACK_PERIOD_SIZE,
-	.period_bytes_max =     PLAYBACK_PERIOD_SIZE,
+	.channels_max =         8,
+	.buffer_bytes_max =     PLAYBACK_NUM_PERIODS * PLAYBACK_MAX_PERIOD_SIZE,
+	.period_bytes_min =	PLAYBACK_MIN_PERIOD_SIZE,
+	.period_bytes_max =     PLAYBACK_MAX_PERIOD_SIZE,
 	.periods_min =          PLAYBACK_NUM_PERIODS,
 	.periods_max =          PLAYBACK_NUM_PERIODS,
 	.fifo_size =            0,
@@ -352,6 +353,17 @@
 	if (ret < 0)
 		pr_info("snd_pcm_hw_constraint_integer failed\n");
 
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		ret = snd_pcm_hw_constraint_minmax(runtime,
+			SNDRV_PCM_HW_PARAM_BUFFER_BYTES,
+			PLAYBACK_NUM_PERIODS * PLAYBACK_MIN_PERIOD_SIZE,
+			PLAYBACK_NUM_PERIODS * PLAYBACK_MAX_PERIOD_SIZE);
+		if (ret < 0) {
+			pr_err("constraint for buffer bytes min max ret = %d\n",
+									ret);
+		}
+	}
+
 	prtd->dsp_cnt = 0;
 	runtime->private_data = prtd;
 
@@ -642,8 +654,8 @@
 
 	ret = q6asm_audio_client_buf_alloc_contiguous(dir,
 			prtd->audio_client,
-			runtime->hw.period_bytes_min,
-			runtime->hw.periods_max);
+			(params_buffer_bytes(params) / params_periods(params)),
+			 params_periods(params));
 	if (ret < 0) {
 		pr_err("Audio Start: Buffer Allocation failed rc = %d\n",
 							ret);
@@ -659,7 +671,7 @@
 	dma_buf->private_data = NULL;
 	dma_buf->area = buf[0].data;
 	dma_buf->addr =  buf[0].phys;
-	dma_buf->bytes = runtime->hw.buffer_bytes_max;
+	dma_buf->bytes = params_buffer_bytes(params);
 	if (!dma_buf->area)
 		return -ENOMEM;
 
diff --git a/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c b/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
index 17c18dd..58300c4 100644
--- a/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
+++ b/sound/soc/msm/qdsp6v2/msm-pcm-routing-v2.c
@@ -50,6 +50,7 @@
 
 static int fm_switch_enable;
 static int fm_pcmrx_switch_enable;
+static int srs_alsa_ctrl_ever_called;
 
 #define INT_RX_VOL_MAX_STEPS 0x2000
 #define INT_RX_VOL_GAIN 0x2000
@@ -112,6 +113,49 @@
 /* This array is indexed by back-end DAI ID defined in msm-pcm-routing.h
  * If new back-end is defined, add new back-end DAI ID at the end of enum
  */
+
+
+union srs_trumedia_params_u {
+	struct srs_trumedia_params srs_params;
+	unsigned short int raw_params[1];
+};
+static union srs_trumedia_params_u msm_srs_trumedia_params[2];
+static int srs_port_id = -1;
+
+static void srs_send_params(int port_id, unsigned int techs,
+		int param_block_idx)
+{
+	/* only send commands to dsp if srs alsa ctrl was used
+	   at least one time */
+	if (!srs_alsa_ctrl_ever_called)
+		return;
+
+	pr_debug("SRS %s: called, port_id = %d, techs flags = %u, paramblockidx %d",
+		__func__, port_id, techs, param_block_idx);
+	/* force all if techs is set to 1 */
+	if (techs == 1)
+		techs = 0xFFFFFFFF;
+
+	if (techs & (1 << SRS_ID_WOWHD))
+		srs_trumedia_open(port_id, SRS_ID_WOWHD,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.wowhd);
+	if (techs & (1 << SRS_ID_CSHP))
+		srs_trumedia_open(port_id, SRS_ID_CSHP,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.cshp);
+	if (techs & (1 << SRS_ID_HPF))
+		srs_trumedia_open(port_id, SRS_ID_HPF,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.hpf);
+	if (techs & (1 << SRS_ID_PEQ))
+		srs_trumedia_open(port_id, SRS_ID_PEQ,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.peq);
+	if (techs & (1 << SRS_ID_HL))
+		srs_trumedia_open(port_id, SRS_ID_HL,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.hl);
+	if (techs & (1 << SRS_ID_GLOBAL))
+		srs_trumedia_open(port_id, SRS_ID_GLOBAL,
+	(void *)&msm_srs_trumedia_params[param_block_idx].srs_params.global);
+}
+
 #define SLIMBUS_EXTPROC_RX AFE_PORT_INVALID
 static struct msm_pcm_routing_bdai_data msm_bedais[MSM_BACKEND_DAI_MAX] = {
 	{ PRIMARY_I2S_RX, 0, 0, 0, 0, 0},
@@ -285,6 +329,8 @@
 
 			payload.copp_ids[payload.num_copps++] =
 				msm_bedais[i].port_id;
+			srs_port_id = msm_bedais[i].port_id;
+			srs_send_params(srs_port_id, 1, 0);
 		}
 	}
 	if (payload.num_copps)
@@ -393,6 +439,8 @@
 
 			msm_pcm_routing_build_matrix(val,
 				fe_dai_map[val][session_type], path_type);
+			srs_port_id = msm_bedais[reg].port_id;
+			srs_send_params(srs_port_id, 1, 0);
 		}
 	} else {
 		if (test_bit(val, &msm_bedais[reg].fe_sessions) &&
@@ -755,6 +803,104 @@
 	return 0;
 }
 
+static int msm_routing_get_srs_trumedia_control(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	ucontrol->value.integer.value[0] = 0;
+	return 0;
+}
+
+static int msm_routing_set_srs_trumedia_control_(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol)
+{
+	unsigned int techs = 0;
+	unsigned short offset, value, max, index;
+
+	srs_alsa_ctrl_ever_called = 1;
+
+	max = sizeof(msm_srs_trumedia_params) >> 1;
+	index = (unsigned short)((ucontrol->value.integer.value[0] &
+			SRS_PARAM_INDEX_MASK) >> 31);
+	if (SRS_CMD_UPLOAD ==
+		(ucontrol->value.integer.value[0] & SRS_CMD_UPLOAD)) {
+		techs = ucontrol->value.integer.value[0] & 0xFF;
+		pr_debug("SRS %s: send params request, flags = %u",
+			__func__, techs);
+		if (srs_port_id >= 0 && techs)
+			srs_send_params(srs_port_id, techs, index);
+		return 0;
+	}
+	offset = (unsigned short)((ucontrol->value.integer.value[0] &
+			SRS_PARAM_OFFSET_MASK) >> 16);
+	value = (unsigned short)(ucontrol->value.integer.value[0] &
+			SRS_PARAM_VALUE_MASK);
+	if (offset < max) {
+		msm_srs_trumedia_params[index].raw_params[offset] = value;
+		pr_debug("SRS %s: index set... (max %d, requested %d, val %d, paramblockidx %d)",
+			__func__, max, offset, value, index);
+	} else {
+		pr_err("SRS %s: index out of bounds! (max %d, requested %d)",
+				__func__, max, offset);
+	}
+	if (offset == 4) {
+		int i;
+		for (i = 0; i < max; i++) {
+			if (i == 0) {
+				pr_debug("SRS %s: global block start",
+						__func__);
+			}
+			if (i ==
+			(sizeof(struct srs_trumedia_params_GLOBAL) >> 1)) {
+				pr_debug("SRS %s: wowhd block start at offset %d word offset %d",
+					__func__, i, i>>1);
+				break;
+			}
+			pr_debug("SRS %s: param_index %d index %d val %d",
+				__func__, index, i,
+				msm_srs_trumedia_params[index].raw_params[i]);
+		}
+	}
+	return 0;
+}
+
+static int msm_routing_set_srs_trumedia_control(struct snd_kcontrol *kcontrol,
+				struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	pr_debug("SRS control normal called");
+	mutex_lock(&routing_lock);
+	srs_port_id = SLIMBUS_0_RX;
+	ret = msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
+static int msm_routing_set_srs_trumedia_control_I2S(
+		struct snd_kcontrol *kcontrol,
+		struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	pr_debug("SRS control I2S called");
+	mutex_lock(&routing_lock);
+	srs_port_id = PRIMARY_I2S_RX;
+	ret = msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
+static int msm_routing_set_srs_trumedia_control_HDMI(
+		struct snd_kcontrol *kcontrol,
+		struct snd_ctl_elem_value *ucontrol) {
+	int ret;
+
+	pr_debug("SRS control HDMI called");
+	mutex_lock(&routing_lock);
+	srs_port_id = HDMI_RX;
+	ret =  msm_routing_set_srs_trumedia_control_(kcontrol, ucontrol);
+	mutex_unlock(&routing_lock);
+	return ret;
+}
+
 static void msm_send_eq_values(int eq_idx)
 {
 	int result;
@@ -1410,6 +1556,67 @@
 	INT_RX_VOL_GAIN, 0, msm_routing_get_compressed_vol_mixer,
 	msm_routing_set_compressed_vol_mixer, compressed_rx_vol_gain),
 };
+
+static const struct snd_kcontrol_new lpa_SRS_trumedia_controls[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS TruMedia",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+			SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_trumedia_control,
+	.put = msm_routing_set_srs_trumedia_control,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
+static const struct snd_kcontrol_new lpa_SRS_trumedia_controls_HDMI[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS TruMedia HDMI",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+			SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_trumedia_control,
+	.put = msm_routing_set_srs_trumedia_control_HDMI,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
+static const struct snd_kcontrol_new lpa_SRS_trumedia_controls_I2S[] = {
+	{.iface = SNDRV_CTL_ELEM_IFACE_MIXER,
+	.name = "SRS TruMedia I2S",
+	.access = SNDRV_CTL_ELEM_ACCESS_TLV_READ |
+		SNDRV_CTL_ELEM_ACCESS_READWRITE,
+	.info = snd_soc_info_volsw, \
+	.get = msm_routing_get_srs_trumedia_control,
+	.put = msm_routing_set_srs_trumedia_control_I2S,
+	.private_value = ((unsigned long)&(struct soc_mixer_control)
+	{.reg = SND_SOC_NOPM,
+	.rreg = SND_SOC_NOPM,
+	.shift = 0,
+	.rshift = 0,
+	.max = 0xFFFFFFFF,
+	.platform_max = 0xFFFFFFFF,
+	.invert = 0
+	})
+	}
+};
+
 static const struct snd_kcontrol_new eq_enable_mixer_controls[] = {
 	SOC_SINGLE_EXT("MultiMedia1 EQ Enable", SND_SOC_NOPM,
 	MSM_FRONTEND_DAI_MULTIMEDIA1, 1, 0, msm_routing_get_eq_enable_mixer,
@@ -2059,6 +2266,8 @@
 	{"BE_OUT", NULL, "SLIMBUS_3_RX"},
 	{"BE_OUT", NULL, "AUX_PCM_RX"},
 	{"AUX_PCM_TX", NULL, "BE_IN"},
+	{"INCALL_RECORD_TX", NULL, "BE_IN"},
+	{"INCALL_RECORD_RX", NULL, "BE_IN"},
 };
 
 static int msm_pcm_routing_hw_params(struct snd_pcm_substream *substream,
@@ -2100,6 +2309,7 @@
 	for_each_set_bit(i, &bedai->fe_sessions, MSM_FRONTEND_DAI_MM_SIZE) {
 		if (fe_dai_map[i][session_type] != INVALID_SESSION)
 			adm_close(bedai->port_id);
+			srs_port_id = -1;
 	}
 
 	bedai->active = 0;
@@ -2165,6 +2375,8 @@
 
 			msm_pcm_routing_build_matrix(i,
 				fe_dai_map[i][session_type], path_type);
+			srs_port_id = bedai->port_id;
+			srs_send_params(srs_port_id, 1, 0);
 		}
 	}
 
@@ -2232,6 +2444,18 @@
 	snd_soc_add_platform_controls(platform,
 				compressed_vol_mixer_controls,
 			ARRAY_SIZE(compressed_vol_mixer_controls));
+
+	snd_soc_add_platform_controls(platform,
+				lpa_SRS_trumedia_controls,
+			ARRAY_SIZE(lpa_SRS_trumedia_controls));
+
+	snd_soc_add_platform_controls(platform,
+				lpa_SRS_trumedia_controls_HDMI,
+			ARRAY_SIZE(lpa_SRS_trumedia_controls_HDMI));
+
+	snd_soc_add_platform_controls(platform,
+				lpa_SRS_trumedia_controls_I2S,
+			ARRAY_SIZE(lpa_SRS_trumedia_controls_I2S));
 	return 0;
 }
 
diff --git a/sound/soc/msm/qdsp6v2/q6adm.c b/sound/soc/msm/qdsp6v2/q6adm.c
index 7267a82..1e0ad9e 100644
--- a/sound/soc/msm/qdsp6v2/q6adm.c
+++ b/sound/soc/msm/qdsp6v2/q6adm.c
@@ -37,20 +37,215 @@
 	atomic_t copp_id[AFE_MAX_PORTS];
 	atomic_t copp_cnt[AFE_MAX_PORTS];
 	atomic_t copp_stat[AFE_MAX_PORTS];
-	u32      mem_map_handle[AFE_MAX_PORTS];
 	wait_queue_head_t wait[AFE_MAX_PORTS];
-};
 
-static struct acdb_cal_block mem_addr_audproc[MAX_AUDPROC_TYPES];
-static struct acdb_cal_block mem_addr_audvol[MAX_AUDPROC_TYPES];
+	struct acdb_cal_block mem_addr_audproc[MAX_AUDPROC_TYPES];
+	struct acdb_cal_block mem_addr_audvol[MAX_AUDPROC_TYPES];
 
 /* 0 - (MAX_AUDPROC_TYPES -1):				audproc handles */
 /* (MAX_AUDPROC_TYPES -1) - (2 * MAX_AUDPROC_TYPES -1):	audvol handles */
-atomic_t mem_map_handles[(2 * MAX_AUDPROC_TYPES)];
-atomic_t mem_map_index;
+	atomic_t mem_map_cal_handles[(2 * MAX_AUDPROC_TYPES)];
+	atomic_t mem_map_cal_index;
+};
 
 static struct adm_ctl			this_adm;
 
+int srs_trumedia_open(int port_id, int srs_tech_id, void *srs_params)
+{
+	struct adm_cmd_set_pp_params_inband_v5 *adm_params = NULL;
+	int ret = 0, sz = 0;
+	int index;
+
+	pr_debug("SRS - %s", __func__);
+	switch (srs_tech_id) {
+	case SRS_ID_GLOBAL: {
+		struct srs_trumedia_params_GLOBAL *glb_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_GLOBAL);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+			sizeof(struct srs_trumedia_params_GLOBAL) +
+			sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS;
+		adm_params->params.param_size =
+				sizeof(struct srs_trumedia_params_GLOBAL);
+		glb_params = (struct srs_trumedia_params_GLOBAL *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(glb_params, srs_params,
+			sizeof(struct srs_trumedia_params_GLOBAL));
+		pr_debug("SRS - %s: Global params - 1 = %x, 2 = %x, 3 = %x, 4 = %x, 5 = %x, 6 = %x, 7 = %x, 8 = %x\n",
+				__func__, (int)glb_params->v1,
+				(int)glb_params->v2, (int)glb_params->v3,
+				(int)glb_params->v4, (int)glb_params->v5,
+				(int)glb_params->v6, (int)glb_params->v7,
+				(int)glb_params->v8);
+		break;
+	}
+	case SRS_ID_WOWHD: {
+		struct srs_trumedia_params_WOWHD *whd_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_WOWHD);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+			sizeof(struct srs_trumedia_params_WOWHD) +
+			sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS_WOWHD;
+		adm_params->params.param_size =
+				sizeof(struct srs_trumedia_params_WOWHD);
+		whd_params = (struct srs_trumedia_params_WOWHD *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(whd_params, srs_params,
+				sizeof(struct srs_trumedia_params_WOWHD));
+		pr_debug("SRS - %s: WOWHD params - 1 = %x, 2 = %x, 3 = %x, 4 = %x, 5 = %x, 6 = %x, 7 = %x, 8 = %x, 9 = %x, 10 = %x, 11 = %x\n",
+			 __func__, (int)whd_params->v1,
+			(int)whd_params->v2, (int)whd_params->v3,
+			(int)whd_params->v4, (int)whd_params->v5,
+			(int)whd_params->v6, (int)whd_params->v7,
+			(int)whd_params->v8, (int)whd_params->v9,
+			(int)whd_params->v10, (int)whd_params->v11);
+		break;
+	}
+	case SRS_ID_CSHP: {
+		struct srs_trumedia_params_CSHP *chp_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_CSHP);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+			sizeof(struct srs_trumedia_params_CSHP) +
+			sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS_CSHP;
+		adm_params->params.param_size =
+				sizeof(struct srs_trumedia_params_CSHP);
+		chp_params = (struct srs_trumedia_params_CSHP *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(chp_params, srs_params,
+				sizeof(struct srs_trumedia_params_CSHP));
+		pr_debug("SRS - %s: CSHP params - 1 = %x, 2 = %x, 3 = %x, 4 = %x, 5 = %x, 6 = %x, 7 = %x, 8 = %x, 9 = %x\n",
+				__func__, (int)chp_params->v1,
+				(int)chp_params->v2, (int)chp_params->v3,
+				(int)chp_params->v4, (int)chp_params->v5,
+				(int)chp_params->v6, (int)chp_params->v7,
+				(int)chp_params->v8, (int)chp_params->v9);
+		break;
+	}
+	case SRS_ID_HPF: {
+		struct srs_trumedia_params_HPF *hpf_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_HPF);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+			sizeof(struct srs_trumedia_params_HPF) +
+			sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS_HPF;
+		adm_params->params.param_size =
+				sizeof(struct srs_trumedia_params_HPF);
+		hpf_params = (struct srs_trumedia_params_HPF *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(hpf_params, srs_params,
+			sizeof(struct srs_trumedia_params_HPF));
+		pr_debug("SRS - %s: HPF params - 1 = %x\n", __func__,
+				(int)hpf_params->v1);
+		break;
+	}
+	case SRS_ID_PEQ: {
+		struct srs_trumedia_params_PEQ *peq_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_PEQ);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+				sizeof(struct srs_trumedia_params_PEQ) +
+				sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS_PEQ;
+		adm_params->params.param_size =
+				sizeof(struct srs_trumedia_params_PEQ);
+		peq_params = (struct srs_trumedia_params_PEQ *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(peq_params, srs_params,
+				sizeof(struct srs_trumedia_params_PEQ));
+		pr_debug("SRS - %s: PEQ params - 1 = %x 2 = %x, 3 = %x, 4 = %x\n",
+			__func__, (int)peq_params->v1,
+			(int)peq_params->v2, (int)peq_params->v3,
+			(int)peq_params->v4);
+		break;
+	}
+	case SRS_ID_HL: {
+		struct srs_trumedia_params_HL *hl_params = NULL;
+		sz = sizeof(struct adm_cmd_set_pp_params_inband_v5) +
+			sizeof(struct srs_trumedia_params_HL);
+		adm_params = kzalloc(sz, GFP_KERNEL);
+		adm_params->payload_size =
+			sizeof(struct srs_trumedia_params_HL) +
+			sizeof(struct adm_param_data_v5);
+		adm_params->params.param_id = SRS_TRUMEDIA_PARAMS_HL;
+		adm_params->params.param_size =
+			sizeof(struct srs_trumedia_params_HL);
+		hl_params = (struct srs_trumedia_params_HL *)
+			((u8 *)adm_params +
+			sizeof(struct adm_cmd_set_pp_params_inband_v5));
+		memcpy(hl_params, srs_params,
+				sizeof(struct srs_trumedia_params_HL));
+		pr_debug("SRS - %s: HL params - 1 = %x, 2 = %x, 3 = %x, 4 = %x, 5 = %x, 6 = %x, 7 = %x\n",
+				__func__, (int)hl_params->v1,
+				(int)hl_params->v2, (int)hl_params->v3,
+				(int)hl_params->v4, (int)hl_params->v5,
+				(int)hl_params->v6, (int)hl_params->v7);
+		break;
+	}
+	default:
+		goto fail_cmd;
+	}
+
+	adm_params->hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+				APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	adm_params->hdr.pkt_size = sz;
+	adm_params->hdr.src_svc = APR_SVC_ADM;
+	adm_params->hdr.src_domain = APR_DOMAIN_APPS;
+	adm_params->hdr.src_port = port_id;
+	adm_params->hdr.dest_svc = APR_SVC_ADM;
+	adm_params->hdr.dest_domain = APR_DOMAIN_ADSP;
+	index = afe_get_port_index(port_id);
+	adm_params->hdr.dest_port = atomic_read(&this_adm.copp_id[index]);
+	adm_params->hdr.token = port_id;
+	adm_params->hdr.opcode = ADM_CMD_SET_PP_PARAMS_V5;
+	adm_params->payload_addr_lsw = 0;
+	adm_params->payload_addr_msw = 0;
+	adm_params->mem_map_handle = 0;
+
+	adm_params->params.module_id = SRS_TRUMEDIA_MODULE_ID;
+	adm_params->params.reserved = 0;
+
+	pr_debug("SRS - %s: Command was sent now check Q6 - port id = %d, size %d, module id %x, param id %x.\n",
+			__func__, adm_params->hdr.dest_port,
+			adm_params->payload_size, adm_params->params.module_id,
+			adm_params->params.param_id);
+
+	ret = apr_send_pkt(this_adm.apr, (uint32_t *)adm_params);
+	if (ret < 0) {
+		pr_err("SRS - %s: ADM enable for port %d failed\n", __func__,
+			port_id);
+		ret = -EINVAL;
+		goto fail_cmd;
+	}
+	/* Wait for the callback with copp id */
+	ret = wait_event_timeout(this_adm.wait[index], 1,
+			msecs_to_jiffies(TIMEOUT_MS));
+	if (!ret) {
+		pr_err("%s: SRS set params timed out port = %d\n",
+			__func__, port_id);
+		ret = -EINVAL;
+		goto fail_cmd;
+	}
+
+fail_cmd:
+	kfree(adm_params);
+	return ret;
+}
+
 static void adm_callback_debug_print(struct apr_client_data *data)
 {
 	uint32_t *payload;
@@ -92,14 +287,14 @@
 		pr_debug("Resetting calibration blocks");
 		for (i = 0; i < MAX_AUDPROC_TYPES; i++) {
 			/* Device calibration */
-			mem_addr_audproc[i].cal_size = 0;
-			mem_addr_audproc[i].cal_kvaddr = 0;
-			mem_addr_audproc[i].cal_paddr = 0;
+			this_adm.mem_addr_audproc[i].cal_size = 0;
+			this_adm.mem_addr_audproc[i].cal_kvaddr = 0;
+			this_adm.mem_addr_audproc[i].cal_paddr = 0;
 
 			/* Volume calibration */
-			mem_addr_audvol[i].cal_size = 0;
-			mem_addr_audvol[i].cal_kvaddr = 0;
-			mem_addr_audvol[i].cal_paddr = 0;
+			this_adm.mem_addr_audvol[i].cal_size = 0;
+			this_adm.mem_addr_audvol[i].cal_kvaddr = 0;
+			this_adm.mem_addr_audvol[i].cal_paddr = 0;
 		}
 		return 0;
 	}
@@ -199,8 +394,9 @@
 		case ADM_CMDRSP_SHARED_MEM_MAP_REGIONS:
 			pr_debug("%s: ADM_CMDRSP_SHARED_MEM_MAP_REGIONS\n",
 				__func__);
-			atomic_set(&mem_map_handles[
-				atomic_read(&mem_map_index)], *payload);
+			atomic_set(&this_adm.mem_map_cal_handles[
+				atomic_read(&this_adm.mem_map_cal_index)],
+				*payload);
 			atomic_set(&this_adm.copp_stat[0], 1);
 			wake_up(&this_adm.wait[index]);
 			break;
@@ -247,8 +443,8 @@
 	adm_params.hdr.opcode = ADM_CMD_SET_PP_PARAMS_V5;
 	adm_params.payload_addr_lsw = aud_cal->cal_paddr;
 	adm_params.payload_addr_msw = 0;
-	adm_params.mem_map_handle = atomic_read(&mem_map_handles[
-					atomic_read(&mem_map_index)]);
+	adm_params.mem_map_handle = atomic_read(&this_adm.mem_map_cal_handles[
+				atomic_read(&this_adm.mem_map_cal_index)]);
 	adm_params.payload_size = aud_cal->cal_size;
 
 	atomic_set(&this_adm.copp_stat[index], 0);
@@ -293,15 +489,16 @@
 	get_audproc_cal(acdb_path, &aud_cal);
 
 	/* map & cache buffers used */
-	atomic_set(&mem_map_index, acdb_path);
-	if (((mem_addr_audproc[acdb_path].cal_paddr != aud_cal.cal_paddr)  &&
-		(aud_cal.cal_size > 0)) ||
-		(aud_cal.cal_size > mem_addr_audproc[acdb_path].cal_size)) {
+	atomic_set(&this_adm.mem_map_cal_index, acdb_path);
+	if (((this_adm.mem_addr_audproc[acdb_path].cal_paddr !=
+		aud_cal.cal_paddr)  && (aud_cal.cal_size > 0)) ||
+		(aud_cal.cal_size >
+		this_adm.mem_addr_audproc[acdb_path].cal_size)) {
 
-		if (mem_addr_audproc[acdb_path].cal_paddr != 0)
+		if (this_adm.mem_addr_audproc[acdb_path].cal_paddr != 0)
 			adm_memory_unmap_regions(port_id,
-				&mem_addr_audproc[acdb_path].cal_paddr,
-				&size, 1);
+				&this_adm.mem_addr_audproc[acdb_path].
+				cal_paddr, &size, 1);
 
 		result = adm_memory_map_regions(port_id, &aud_cal.cal_paddr,
 						0, &size, 1);
@@ -310,9 +507,9 @@
 				acdb_path, aud_cal.cal_paddr,
 				aud_cal.cal_size);
 		} else {
-			mem_addr_audproc[acdb_path].cal_paddr =
+			this_adm.mem_addr_audproc[acdb_path].cal_paddr =
 							aud_cal.cal_paddr;
-			mem_addr_audproc[acdb_path].cal_size = size;
+			this_adm.mem_addr_audproc[acdb_path].cal_size = size;
 		}
 	}
 
@@ -327,14 +524,16 @@
 	get_audvol_cal(acdb_path, &aud_cal);
 
 	/* map & cache buffers used */
-	atomic_set(&mem_map_index, (acdb_path + MAX_AUDPROC_TYPES));
-	if (((mem_addr_audvol[acdb_path].cal_paddr != aud_cal.cal_paddr)  &&
-		(aud_cal.cal_size > 0))  ||
-		(aud_cal.cal_size > mem_addr_audvol[acdb_path].cal_size)) {
+	atomic_set(&this_adm.mem_map_cal_index,
+		(acdb_path + MAX_AUDPROC_TYPES));
+	if (((this_adm.mem_addr_audvol[acdb_path].cal_paddr !=
+		aud_cal.cal_paddr)  && (aud_cal.cal_size > 0))  ||
+		(aud_cal.cal_size >
+		this_adm.mem_addr_audvol[acdb_path].cal_size)) {
 
-		if (mem_addr_audvol[acdb_path].cal_paddr != 0)
+		if (this_adm.mem_addr_audvol[acdb_path].cal_paddr != 0)
 			adm_memory_unmap_regions(port_id,
-				&mem_addr_audvol[acdb_path].cal_paddr,
+				&this_adm.mem_addr_audvol[acdb_path].cal_paddr,
 				&size, 1);
 
 		result = adm_memory_map_regions(port_id, &aud_cal.cal_paddr,
@@ -344,9 +543,9 @@
 				acdb_path, aud_cal.cal_paddr,
 				aud_cal.cal_size);
 		} else {
-			mem_addr_audvol[acdb_path].cal_paddr =
+			this_adm.mem_addr_audvol[acdb_path].cal_paddr =
 							aud_cal.cal_paddr;
-			mem_addr_audvol[acdb_path].cal_size = size;
+			this_adm.mem_addr_audvol[acdb_path].cal_size = size;
 		}
 	}
 
@@ -535,8 +734,8 @@
 			open.dev_channel_mapping[3] = PCM_CHANNEL_FC;
 			open.dev_channel_mapping[4] = PCM_CHANNEL_LB;
 			open.dev_channel_mapping[5] = PCM_CHANNEL_RB;
-			open.dev_channel_mapping[6] = PCM_CHANNEL_FLC;
-			open.dev_channel_mapping[7] = PCM_CHANNEL_FRC;
+			open.dev_channel_mapping[6] = PCM_CHANNEL_RLC;
+			open.dev_channel_mapping[7] = PCM_CHANNEL_RRC;
 		} else {
 			pr_err("%s invalid num_chan %d\n", __func__,
 					channel_mode);
@@ -817,8 +1016,8 @@
 	unmap_regions.hdr.dest_port = atomic_read(&this_adm.copp_id[index]);
 	unmap_regions.hdr.token = port_id;
 	unmap_regions.hdr.opcode = ADM_CMD_SHARED_MEM_UNMAP_REGIONS;
-	unmap_regions.mem_map_handle = atomic_read(&mem_map_handles[
-						atomic_read(&mem_map_index)]);
+	unmap_regions.mem_map_handle = atomic_read(&this_adm.
+		mem_map_cal_handles[atomic_read(&this_adm.mem_map_cal_index)]);
 	atomic_set(&this_adm.copp_stat[0], 0);
 	ret = apr_send_pkt(this_adm.apr, (uint32_t *) &unmap_regions);
 	if (ret < 0) {
diff --git a/sound/soc/msm/qdsp6v2/q6afe.c b/sound/soc/msm/qdsp6v2/q6afe.c
index de9841a..9387d21 100644
--- a/sound/soc/msm/qdsp6v2/q6afe.c
+++ b/sound/soc/msm/qdsp6v2/q6afe.c
@@ -39,12 +39,14 @@
 	void *tx_private_data;
 	void *rx_private_data;
 	uint32_t mmap_handle;
+
+	struct acdb_cal_block afe_cal_addr[MAX_AUDPROC_TYPES];
+	atomic_t mem_map_cal_handles[MAX_AUDPROC_TYPES];
+	atomic_t mem_map_cal_index;
 };
 
 static struct afe_ctl this_afe;
 
-static struct acdb_cal_block afe_cal_addr[MAX_AUDPROC_TYPES];
-
 #define TIMEOUT_MS 1000
 #define Q6AFE_MAX_VOLUME 0x3FFF
 
@@ -113,7 +115,13 @@
 				AFE_SERVICE_CMDRSP_SHARED_MEM_MAP_REGIONS) {
 			pr_debug("%s: mmap_handle: 0x%x\n",
 						__func__, payload[0]);
-			this_afe.mmap_handle = (uint32_t)payload[0];
+			if (atomic_read(&this_afe.mem_map_cal_index) != -1)
+				atomic_set(&this_afe.mem_map_cal_handles[
+					atomic_read(
+					&this_afe.mem_map_cal_index)],
+					(uint32_t)payload[0]);
+			else
+				this_afe.mmap_handle = (uint32_t)payload[0];
 			atomic_set(&this_afe.state, 0);
 			wake_up(&this_afe.wait[data->token]);
 		} else if (data->opcode == AFE_EVENT_RT_PROXY_PORT_STATUS) {
@@ -219,6 +227,10 @@
 	case SLIMBUS_1_TX:
 		ret_size = SIZEOF_CFG_CMD(afe_param_id_slimbus_cfg);
 		break;
+	case VOICE_RECORD_RX:
+	case VOICE_RECORD_TX:
+		ret_size = SIZEOF_CFG_CMD(afe_param_id_pseudo_port_cfg);
+		break;
 	case RT_PROXY_PORT_001_RX:
 	case RT_PROXY_PORT_001_TX:
 		ret_size = SIZEOF_CFG_CMD(afe_param_id_rt_proxy_port_cfg);
@@ -248,9 +260,77 @@
 	}
 	return ret;
 }
+
 static void afe_send_cal_block(int32_t path, u16 port_id)
 {
-	/* To come back */
+	int						result = 0;
+	int						index = 0;
+	int						size = 4096;
+	struct acdb_cal_block				cal_block;
+	struct afe_audioif_config_command_no_payload	afe_cal;
+	pr_debug("%s: path %d\n", __func__, path);
+
+	get_afe_cal(path, &cal_block);
+	if (cal_block.cal_size <= 0) {
+		pr_debug("%s: No AFE cal to send!\n", __func__);
+		goto done;
+	}
+
+	if ((this_afe.afe_cal_addr[path].cal_paddr != cal_block.cal_paddr) ||
+		(cal_block.cal_size > this_afe.afe_cal_addr[path].cal_size)) {
+		atomic_set(&this_afe.mem_map_cal_index, path);
+		if (this_afe.afe_cal_addr[path].cal_paddr != 0)
+			afe_cmd_memory_unmap(
+				this_afe.afe_cal_addr[path].cal_paddr);
+
+		afe_cmd_memory_map(cal_block.cal_paddr, size);
+		atomic_set(&this_afe.mem_map_cal_index, -1);
+		this_afe.afe_cal_addr[path].cal_paddr = cal_block.cal_paddr;
+		this_afe.afe_cal_addr[path].cal_size = size;
+	}
+
+	index = q6audio_get_port_index(port_id);
+	if (index < 0) {
+		pr_debug("%s: AFE port index invalid!\n", __func__);
+		goto done;
+	}
+
+	afe_cal.hdr.hdr_field = APR_HDR_FIELD(APR_MSG_TYPE_SEQ_CMD,
+				APR_HDR_LEN(APR_HDR_SIZE), APR_PKT_VER);
+	afe_cal.hdr.pkt_size = sizeof(afe_cal);
+	afe_cal.hdr.src_port = 0;
+	afe_cal.hdr.dest_port = 0;
+	afe_cal.hdr.token = index;
+	afe_cal.hdr.opcode = AFE_PORT_CMD_SET_PARAM_V2;
+	afe_cal.param.port_id = port_id;
+	afe_cal.param.payload_size = cal_block.cal_size;
+	afe_cal.param.payload_address_lsw = cal_block.cal_paddr;
+	afe_cal.param.payload_address_msw = 0;
+	afe_cal.param.mem_map_handle =
+			atomic_read(&this_afe.mem_map_cal_handles[path]);
+
+	pr_debug("%s: AFE cal sent for device port = %d, path = %d, cal size = %d, cal addr = 0x%x\n",
+		__func__, port_id, path,
+		cal_block.cal_size, cal_block.cal_paddr);
+
+	atomic_set(&this_afe.state, 1);
+	result = apr_send_pkt(this_afe.apr, (uint32_t *) &afe_cal);
+	if (result < 0) {
+		pr_err("%s: AFE cal for port %d failed\n",
+			__func__, port_id);
+	}
+
+	result = wait_event_timeout(this_afe.wait[index],
+				 (atomic_read(&this_afe.state) == 0),
+				 msecs_to_jiffies(TIMEOUT_MS));
+	if (!result) {
+		pr_err("%s: wait_event timeout SET AFE CAL\n", __func__);
+		goto done;
+	}
+
+	pr_debug("%s: AFE cal sent for path %d device!\n", __func__, path);
+done:
+	return;
 }
 
 void afe_send_cal(u16 port_id)
@@ -334,6 +414,10 @@
 	case HDMI_RX:
 		cfg_type = AFE_PARAM_ID_HDMI_CONFIG;
 		break;
+	case VOICE_RECORD_RX:
+	case VOICE_RECORD_TX:
+		cfg_type = AFE_PARAM_ID_PSEUDO_PORT_CONFIG;
+		break;
 	case SLIMBUS_0_RX:
 	case SLIMBUS_0_TX:
 	case SLIMBUS_1_RX:
@@ -1933,6 +2017,7 @@
 	int i = 0;
 	atomic_set(&this_afe.state, 0);
 	atomic_set(&this_afe.status, 0);
+	atomic_set(&this_afe.mem_map_cal_index, -1);
 	this_afe.apr = NULL;
 	this_afe.mmap_handle = 0;
 	for (i = 0; i < AFE_MAX_PORTS; i++)
@@ -1948,9 +2033,9 @@
 
 	config_debug_fs_exit();
 	for (i = 0; i < MAX_AUDPROC_TYPES; i++) {
-		if (afe_cal_addr[i].cal_paddr != 0)
+		if (this_afe.afe_cal_addr[i].cal_paddr != 0)
 			afe_cmd_memory_unmap_nowait(
-				afe_cal_addr[i].cal_paddr);
+				this_afe.afe_cal_addr[i].cal_paddr);
 	}
 }
 
diff --git a/sound/soc/msm/qdsp6v2/q6asm.c b/sound/soc/msm/qdsp6v2/q6asm.c
index 0ddaabe..0cbd136 100644
--- a/sound/soc/msm/qdsp6v2/q6asm.c
+++ b/sound/soc/msm/qdsp6v2/q6asm.c
@@ -558,6 +558,7 @@
 		spin_lock_init(&ac->port[lcnt].dsp_lock);
 	}
 	atomic_set(&ac->cmd_state, 0);
+	atomic_set(&ac->nowait_cmd_cnt, 0);
 
 	pr_debug("%s: session[%d]\n", __func__, ac->session);
 
@@ -889,6 +890,25 @@
 	return 0;
 }
 
+static int32_t is_no_wait_cmd_rsp(uint32_t opcode, uint32_t *cmd_type)
+{
+	if (opcode == APR_BASIC_RSP_RESULT) {
+		if (cmd_type != NULL) {
+			switch (cmd_type[0]) {
+			case ASM_SESSION_CMD_RUN_V2:
+			case ASM_SESSION_CMD_PAUSE:
+			case ASM_DATA_CMD_EOS:
+				return 1;
+			default:
+				break;
+			}
+		} else
+			pr_err("%s: null pointer!", __func__);
+	} else if (opcode == ASM_DATA_EVENT_RENDERED_EOS)
+		return 1;
+
+	return 0;
+}
 
 static int32_t q6asm_callback(struct apr_client_data *data, void *priv)
 {
@@ -897,6 +917,7 @@
 	uint32_t token;
 	unsigned long dsp_flags;
 	uint32_t *payload;
+	uint32_t wakeup_flag = 1;
 
 
 	if ((ac == NULL) || (data == NULL)) {
@@ -910,6 +931,14 @@
 	}
 
 	payload = data->payload;
+	if ((atomic_read(&ac->nowait_cmd_cnt) > 0) &&
+		is_no_wait_cmd_rsp(data->opcode, payload)) {
+		pr_debug("%s: nowait_cmd_cnt %d\n",
+				__func__,
+				atomic_read(&ac->nowait_cmd_cnt));
+		atomic_dec(&ac->nowait_cmd_cnt);
+		wakeup_flag = 0;
+	}
 
 	if (data->opcode == RESET_EVENTS) {
 		pr_debug("q6asm_callback: Reset event is received: %d %d apr[%p]\n",
@@ -961,7 +990,7 @@
 		case ASM_STREAM_CMD_SET_ENCDEC_PARAM:
 		pr_debug("%s:Payload = [0x%x]stat[0x%x]\n",
 				__func__, payload[0], payload[1]);
-			if (atomic_read(&ac->cmd_state)) {
+			if (atomic_read(&ac->cmd_state) && wakeup_flag) {
 				atomic_set(&ac->cmd_state, 0);
 				wake_up(&ac->cmd_wait);
 			}
@@ -1605,6 +1634,7 @@
 		pr_err("%s:Commmand run failed[%d]", __func__, rc);
 		return -EINVAL;
 	}
+	atomic_inc(&ac->nowait_cmd_cnt);
 	return 0;
 }
 
@@ -1819,27 +1849,28 @@
 		lchannel_mapping[3] = PCM_CHANNEL_LB;
 		lchannel_mapping[4] = PCM_CHANNEL_RB;
 	} else if (channels == 6) {
+		channel_mapping[0] = PCM_CHANNEL_FC;
+		channel_mapping[1] = PCM_CHANNEL_FL;
+		channel_mapping[2] = PCM_CHANNEL_FR;
+		channel_mapping[3] = PCM_CHANNEL_LB;
+		channel_mapping[4] = PCM_CHANNEL_RB;
+		channel_mapping[5] = PCM_CHANNEL_LFE;
+	} else if (channels == 8) {
 		lchannel_mapping[0] = PCM_CHANNEL_FL;
 		lchannel_mapping[1] = PCM_CHANNEL_FR;
 		lchannel_mapping[2] = PCM_CHANNEL_LFE;
 		lchannel_mapping[3] = PCM_CHANNEL_FC;
 		lchannel_mapping[4] = PCM_CHANNEL_LB;
 		lchannel_mapping[5] = PCM_CHANNEL_RB;
-	} else if (channels == 6) {
-		lchannel_mapping[0] = PCM_CHANNEL_FL;
-		lchannel_mapping[1] = PCM_CHANNEL_FR;
-		lchannel_mapping[2] = PCM_CHANNEL_LFE;
-		lchannel_mapping[3] = PCM_CHANNEL_FC;
-		lchannel_mapping[4] = PCM_CHANNEL_LB;
-		lchannel_mapping[5] = PCM_CHANNEL_RB;
-		lchannel_mapping[6] = PCM_CHANNEL_FLC;
-		lchannel_mapping[7] = PCM_CHANNEL_FRC;
+		lchannel_mapping[6] = PCM_CHANNEL_RLC;
+		lchannel_mapping[7] = PCM_CHANNEL_RRC;
 	} else {
 		pr_err("%s: ERROR.unsupported num_ch = %u\n",
 		 __func__, channels);
 		return -EINVAL;
 	}
 	return 0;
+
 }
 
 int q6asm_enable_sbrps(struct audio_client *ac,
@@ -3380,6 +3411,7 @@
 		pr_err("%s:Commmand 0x%x failed\n", __func__, hdr.opcode);
 		goto fail_cmd;
 	}
+	atomic_inc(&ac->nowait_cmd_cnt);
 	return 0;
 fail_cmd:
 	return -EINVAL;
diff --git a/sound/soc/msm/qdsp6v2/q6voice.c b/sound/soc/msm/qdsp6v2/q6voice.c
index 263f47f..349fcf2 100644
--- a/sound/soc/msm/qdsp6v2/q6voice.c
+++ b/sound/soc/msm/qdsp6v2/q6voice.c
@@ -2928,23 +2928,25 @@
 		cvs_start_record.hdr.src_port = v->session_id;
 		cvs_start_record.hdr.dest_port = cvs_handle;
 		cvs_start_record.hdr.token = 0;
-		cvs_start_record.hdr.opcode = VSS_ISTREAM_CMD_START_RECORD;
+		cvs_start_record.hdr.opcode = VSS_IRECORD_CMD_START;
 
+		cvs_start_record.rec_mode.port_id =
+					VSS_IRECORD_PORT_ID_DEFAULT;
 		if (rec_mode == VOC_REC_UPLINK) {
 			cvs_start_record.rec_mode.rx_tap_point =
-						VSS_TAP_POINT_NONE;
+					VSS_IRECORD_TAP_POINT_NONE;
 			cvs_start_record.rec_mode.tx_tap_point =
-						VSS_TAP_POINT_STREAM_END;
+					VSS_IRECORD_TAP_POINT_STREAM_END;
 		} else if (rec_mode == VOC_REC_DOWNLINK) {
 			cvs_start_record.rec_mode.rx_tap_point =
-						VSS_TAP_POINT_STREAM_END;
+					VSS_IRECORD_TAP_POINT_STREAM_END;
 			cvs_start_record.rec_mode.tx_tap_point =
-						VSS_TAP_POINT_NONE;
+					VSS_IRECORD_TAP_POINT_NONE;
 		} else if (rec_mode == VOC_REC_BOTH) {
 			cvs_start_record.rec_mode.rx_tap_point =
-						VSS_TAP_POINT_STREAM_END;
+					VSS_IRECORD_TAP_POINT_STREAM_END;
 			cvs_start_record.rec_mode.tx_tap_point =
-						VSS_TAP_POINT_STREAM_END;
+					VSS_IRECORD_TAP_POINT_STREAM_END;
 		} else {
 			pr_err("%s: Invalid in-call rec_mode %d\n", __func__,
 				rec_mode);
@@ -3011,7 +3013,7 @@
 		cvs_stop_record.src_port = v->session_id;
 		cvs_stop_record.dest_port = cvs_handle;
 		cvs_stop_record.token = 0;
-		cvs_stop_record.opcode = VSS_ISTREAM_CMD_STOP_RECORD;
+		cvs_stop_record.opcode = VSS_IRECORD_CMD_STOP;
 
 		v->cvs_state = CMD_STATUS_FAIL;
 
@@ -4166,8 +4168,8 @@
 			case VSS_ICOMMON_CMD_SET_UI_PROPERTY:
 			case VSS_ISTREAM_CMD_START_PLAYBACK:
 			case VSS_ISTREAM_CMD_STOP_PLAYBACK:
-			case VSS_ISTREAM_CMD_START_RECORD:
-			case VSS_ISTREAM_CMD_STOP_RECORD:
+			case VSS_IRECORD_CMD_START:
+			case VSS_IRECORD_CMD_STOP:
 			case VSS_ISTREAM_CMD_SET_PACKET_EXCHANGE_MODE:
 			case VSS_ISTREAM_CMD_SET_OOB_PACKET_EXCHANGE_CONFIG:
 				pr_debug("%s: cmd = 0x%x\n", __func__, ptr[0]);
diff --git a/sound/soc/msm/qdsp6v2/q6voice.h b/sound/soc/msm/qdsp6v2/q6voice.h
index 6fb4b04..d19697a 100644
--- a/sound/soc/msm/qdsp6v2/q6voice.h
+++ b/sound/soc/msm/qdsp6v2/q6voice.h
@@ -495,17 +495,26 @@
 #define VSS_ISTREAM_CMD_STOP_PLAYBACK                   0x00011239
 /* Stop the in-call music delivery on the Tx voice path. */
 
-#define VSS_ISTREAM_CMD_START_RECORD                    0x00011236
+#define VSS_IRECORD_CMD_START				0x000112BE
 /* Start in-call conversation recording. */
-#define VSS_ISTREAM_CMD_STOP_RECORD                     0x00011237
+#define VSS_IRECORD_CMD_STOP				0x00011237
 /* Stop in-call conversation recording. */
 
-#define VSS_TAP_POINT_NONE                              0x00010F78
+#define VSS_IRECORD_PORT_ID_DEFAULT			0x0000FFFF
+/* Default AFE port ID. */
+
+#define VSS_IRECORD_TAP_POINT_NONE			0x00010F78
 /* Indicates no tapping for specified path. */
 
-#define VSS_TAP_POINT_STREAM_END                        0x00010F79
+#define VSS_IRECORD_TAP_POINT_STREAM_END		0x00010F79
 /* Indicates that specified path should be tapped at the end of the stream. */
 
+#define VSS_IRECORD_MODE_TX_RX_STEREO			0x00010F7A
+/* Select Tx on left channel and Rx on right channel. */
+
+#define VSS_IRECORD_MODE_TX_RX_MIXING			0x00010F7B
+/* Select mixed Tx and Rx paths. */
+
 #define VSS_ISTREAM_EVT_NOT_READY			0x000110FD
 
 #define VSS_ISTREAM_EVT_READY				0x000110FC
@@ -529,16 +538,30 @@
 
 #define VSS_ISTREAM_CMD_SET_PACKET_EXCHANGE_MODE	0x0001136A
 
-struct vss_istream_cmd_start_record_t {
+struct vss_irecord_cmd_start_t {
 	uint32_t rx_tap_point;
 	/* Tap point to use on the Rx path. Supported values are:
-	 * VSS_TAP_POINT_NONE : Do not record Rx path.
-	 * VSS_TAP_POINT_STREAM_END : Rx tap point is at the end of the stream.
+	 * VSS_IRECORD_TAP_POINT_NONE : Do not record Rx path.
+	 * VSS_IRECORD_TAP_POINT_STREAM_END : Rx tap point is at the end of
+	 * the stream.
 	 */
 	uint32_t tx_tap_point;
 	/* Tap point to use on the Tx path. Supported values are:
-	 * VSS_TAP_POINT_NONE : Do not record tx path.
-	 * VSS_TAP_POINT_STREAM_END : Tx tap point is at the end of the stream.
+	 * VSS_IRECORD_TAP_POINT_NONE : Do not record tx path.
+	 * VSS_IRECORD_TAP_POINT_STREAM_END : Tx tap point is at the end of
+	 * the stream.
+	 */
+	uint16_t port_id;
+	/* AFE Port ID to whcih the conversation recording stream needs to be
+	 * sent. Set this to #VSS_IRECORD_PORT_ID_DEFAULT to use default AFE
+	 * pseudo ports (0x8003 for Rx and 0x8004 for Tx).
+	 */
+	uint32_t mode;
+	/* Recording Mode. The mode parameter value is ignored if the port_id
+	 * is set to #VSS_IRECORD_PORT_ID_DEFAULT.
+	 * The supported values:
+	 * #VSS_IRECORD_MODE_TX_RX_STEREO
+	 * #VSS_IRECORD_MODE_TX_RX_MIXING
 	 */
 } __packed;
 
@@ -782,7 +805,7 @@
 } __packed;
 struct cvs_start_record_cmd {
 	struct apr_hdr hdr;
-	struct vss_istream_cmd_start_record_t rec_mode;
+	struct vss_irecord_cmd_start_t rec_mode;
 } __packed;
 
 struct cvs_dec_buffer_ready_cmd {